trajectory_msgsを使ってガゼボとコントローラを公開する

Question

私はロボットアームで作業しています。まず、アームの物理モデルをURDFファイルで書きました。このモデルはRvizとGazeboで動作します。また、私は（すべてのロボットの関節のコントロールのために）controllers.yamlファイルを作成し、私はコマンドを使用する場合：

rostopicパブ/ arm_controller /コマンドtrajectory_msgs/JointTrajectory「{joint_names：[ "ヒップ"、 "肩" 、 "肘"、 "手首"]、ポイント：[{位置：[0.1、-0.5,0.5,0.75]、time_from_start：[1.0,0.0]}]} -1

両方のモデル（上rivzとgazebo）は同時に動く。 しかし、trajectory_msgs :: JointTrajectoryを使用してアームを独立に動かすための.cppファイルを作成します。私は私のfile.launchを起動し、私は私のCPPファイルをrosrunすると、両方がrqt_graphに

#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; 
} 

に接続されています。ここに私のcppファイルです。しかしすぐに、私は（発射端末上）エラーがあります：

[ERROR] [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

私は私の問題を解決しました。ここで働くというのが私の最初の例であり、私は後にそれを説明する：あなたは2つのコードを比較すると

#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; 
} 

、私は4に「traj.points.resize（）」（最初の1に）初期化さでした間違っているのは、すべてのポイントが1つの親または1つの子供で互いに接続されているからです。だから、私は1つのウェイポイント（私は2ロボットの腕を持っている場合、私は2つのウェイポイントを持っているだろう...）とこの方法ポイントは4つの位置（腰、肩、肘、手首） 。さらに、私は "traj.points [0] .positions"を初期化して4（関節の数）にリサイズするのを忘れていました。 最後に、「time_from_start = ros :: Duration（1）」は、ロボットアームの動きの速度であるため、読み取るので増分する必要はありません。