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

Publishing between gazebo and controller using trajectory_msgs

我正在研究机器人手臂,首先,我使用 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。因此,我认为我的代码(或启动代码)有问题,但我不知道在哪里。

有谁知道哪里出了问题?谢谢。

我解决了我的问题。这是我的第一个工作示例,之后我将对其进行解释:

#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)"不需要递增,正如我所读的,因为它是机械臂运动的速度。