使用trajectory_msgs在凉亭和控制器之间发布

Question

我正在研究机器人手臂，首先，我用URDF文件编写了手臂的物理模型。这个模型与Rviz和Gazebo一起工作。此外，我创建了一个controllers.yaml文件（控制所有机器人的关节），当我使用命令：

rostopic酒馆/ arm_controller /命令trajectory_msgs/JointTrajectory“{joint_names：[ “臀部”， “肩” ，“肘”，“手腕”]，点：[{position：[0.1，-0.5,0.5,0.75]，time_from_start：[1.0,0.0]}]}'-1

both models rivz和凉亭）同时移动。 但现在，我想创建一个.cpp文件，让arm通过使用trajectory_msgs :: JointTrajectory独立移动。这是我的CPP文件：

#include <ros/ros.h> 
#include <trajectory_msgs/JointTrajectory.h> 

int main(int argc, char** argv) { 
ros::init(argc, argv, "state_publisher"); 
ros::NodeHandle n; 

ros::Publisher arm_pub = n.advertise<trajectory_msgs::JointTrajectory>("/arm_controller/command",1); 
trajectory_msgs::JointTrajectory traj; 

traj.header.stamp = ros::Time::now(); 
traj.header.frame_id = "base_link"; 
traj.joint_names.resize(4); 
traj.points.resize(4); 

traj.joint_names[0] ="hip"; 
traj.joint_names[1] ="shoulder"; 
traj.joint_names[2] ="elbow"; 
traj.joint_names[3] ="wrist"; 

double dt(0.5); 

while (ros::ok()) { 

    for(int i=0;i<4;i++){ 

    double x1 = cos(i); 
    trajectory_msgs::JointTrajectoryPoint points_n; 
    points_n.positions.push_back(x1); 
    points_n.positions.push_back(x1); 
    points_n.positions.push_back(x1); 
    points_n.positions.push_back(x1); 
    traj.points.push_back(points_n); 

    traj.points[i].time_from_start = ros::Duration(dt*i); 

    } 

    arm_pub.publish(traj); 
    ros::spinOnce(); 
} 

return 0; 
} 

当我启动我的file.launch和我rosrun我的cpp文件，都连接上rqt_graph。但立即，我有错误（发射终端上）：

[错误] [1497596211.214814221，9.889000000]：轨迹信息中包含路点未严格时间增加。

实际上，当我使用命令rostopic回声/ arm_controller /命令，我有：

positions: [0.0, 1.0, 0.0, 2.0] 
velocities: [] 
accelerations: [] 
effort: [] 
time_from_start: 
secs: 0 
nsecs: 0 

的time_from_start始终是0。所以，我觉得我在我的代码已经有问题（或在我的启动代码中）但我不知道在哪里。

有没有人有一个想法是什么错？谢谢。

Answer 1

我解决了我的问题。这是我的第一个例子是工作后，我解释一下：

#include <ros/ros.h> 
#include <trajectory_msgs/JointTrajectory.h> 
#include "ros/time.h" 

ros::Publisher arm_pub; 

int setValeurPoint(trajectory_msgs::JointTrajectory* trajectoire,int pos_tab, int val); 

int main(int argc, char** argv) { 
    ros::init(argc, argv, "state_publisher"); 
    ros::NodeHandle n; 
    arm_pub = n.advertise<trajectory_msgs::JointTrajectory>("/arm_controller/command",1); 
    ros::Rate loop_rate(10); 

    trajectory_msgs::JointTrajectory traj; 
    trajectory_msgs::JointTrajectoryPoint points_n; 

    traj.header.frame_id = "base_link"; 
    traj.joint_names.resize(4); 
    traj.points.resize(1); 

    traj.points[0].positions.resize(4); 

    traj.joint_names[0] ="hip"; 
    traj.joint_names[1] ="shoulder"; 
    traj.joint_names[2] ="elbow"; 
    traj.joint_names[3] ="wrist"; 

    int i(100); 

    while(ros::ok()) { 

      traj.header.stamp = ros::Time::now(); 

      for(int j=0; j<4; j++) { 
        setValeurPoint(&traj,j,i); 
      } 

      traj.points[0].time_from_start = ros::Duration(1); 

      arm_pub.publish(traj); 
      ros::spinOnce(); 

      loop_rate.sleep(); 
      i++; 
    } 

    return 0; 
} 

int setValeurPoint(trajectory_msgs::JointTrajectory* trajectoire,int pos_tab, int val){ 
    trajectoire->points[0].positions[pos_tab] = val; 
    return 0; 
} 

如果你比较两个代码，我初始化（在第一个）“traj.points.resize（）” 4。这是一个因为所有点都与一个父母或一个孩子相互连接。所以，我只有1个方法点（如果我有2个机器人手臂，我将有2个方向点......），这种方式 - 点由4个位置（臀部，肩膀，肘部，手腕）定义， 。此外，我忘记初始化并将“traj.points [0] .positions”调整为4（关节数量）。 最后，“time_from_start = ros :: Duration（1）”不需要增加，因为我读它是因为它是机器人手臂运动的速度。