Question

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

rostopic pub / arm_controller / command trajectory_msgs / JointTrajectory'{joint_names：[“hip”，“shoulder”，“elbow”，“wrist”]，分：[{position：[0.1，-0.5,0.5,0.75] ，time_from_start：[1.0,0.0]}]}' - 1

两个模型（在rivz和gazebo上）同时移动。但是现在，我想通过使用trajectory_msgs :: JointTrajectory为手臂创建一个.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并且我运行了我的cpp文件时，两者都连接在rqt_graph上。但是立刻，我有一个错误（在发射终端上）：

[错误] [1497596211.214814221,9.889000000]：轨迹消息包含未严格增加的航点。

实际上，当我使用命令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在凉亭和控制器之间发布

1 个答案: