c++ - 使用trajectory_msgs在凉亭和 Controller 之间发布

我正在使用机械臂，首先，我用URDF文件编写了机械臂的物理模型。该模型与Rviz和Gazebo一起使用。此外，当我使用命令时，我创建了一个controllers.yaml文件(用于控制所有机器人的关节):



两个模型(在rivz和凉亭上)同时移动。
但是现在，我想使用trajectory_msgs::JointTrajectory为 ARM 创建一个.cpp文件以独立移动。这是我的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上。但立即，我有一个错误(在启动终端上):



实际上，当我使用命令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个机器人 ARM ，我将有2个航路点...)，并且此航路点由4个位置(臀部，肩膀，肘部，腕部)定义。而且，我忘记了将“traj.points [0] .positions”初始化和调整为4(关节数)的大小。
最后，正如我读到的那样，“time_from_start = ros::Duration(1)”不需要增加，因为它是机器人 ARM 运动的速度。

关于c++ - 使用trajectory_msgs在凉亭和 Controller 之间发布，我们在Stack Overflow上找到一个类似的问题：https://stackoverflow.com/questions/44626435/