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

Question

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

rostopic pub /arm_controller/command trajectory_msgs/JointTrajectory '{joint_names: ["hip","shoulder","elbow","wrist"], points: [{positions:[0.1,-0.5,0.5,0.75], time_from_start: [1.0,0.0]}]}' -1

两个模型（在 rivz 和 gazebo 上）同时移动。但是现在，我想为手臂创建一个 .cpp 文件，以便使用 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。但是立即，我有一个错误（在启动终端上）：

[ERROR] [1497596211.214814221, 9.889000000]: Trajectory message contains waypoints that are not strictly increasing in time.

实际上，当我使用命令 rostopic echo /arm_controller/command 时，我有：

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 个父项或 1 个子项相互连接。所以，我只有 1 个路点（如果我有 2 个机器人手臂，我将有 2 个路点......）并且这个路点由 4 个位置（臀部，肩部，肘部，手腕）定义.此外，我忘记了初始化 "traj.points[0].positions" 并将其调整为 4（关节数）。最后，"time_from_start = ros::Duration(1)"不需要递增，正如我所读的，因为它是机械臂运动的速度。

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

Publishing between gazebo and controller using trajectory_msgs

c++

ros

robotics