使用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）」不需要增加，因爲我讀它是因爲它是機器人手臂運動的速度。