Publication entre gazebo et contrôleur utilisant trajectory_msgs

Question

Je travaille sur un bras de robot, d'abord, j'ai écrit un modèle physique du bras avec un fichier URDF. Ce modèle fonctionne avec Rviz et Gazebo. De plus, j'ai créé un fichier controllers.yaml (pour les contrôles tous les joints du robot) et quand j'utilise la commande:

pub rostopic/arm_controller/commande trajectory_msgs/JointTrajectory « {joint_names: [ "hip", "épaule" , "coude", "poignet"], points: [{positions: [0,1, -0.5,0.5,0.75], time_from_start: [1.0,0.0]}]} » -1

les deux modèles (sur rivz et gazebo) se déplacent simultanément. Mais maintenant, je veux créer un fichier .cpp pour que le bras puisse se déplacer indépendamment en utilisant trajectory_msgs :: JointTrajectory. Voici mon fichier 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; 
} 

Quand je lance mon file.launch et je rosrun mon fichier cpp, les deux sont connectés sur rqt_graph. Mais immédiatement, j'ai une erreur (sur la borne de lancement):

[ERREUR] [1497596211,214814221, 9,889000000]: message Trajectoire contient waypoints qui augmentent pas strictement dans le temps.

En effet, quand j'utilise la commande rostopic echo/arm_controller/commande, j'ai:

positions: [0.0, 1.0, 0.0, 2.0] 
velocities: [] 
accelerations: [] 
effort: [] 
time_from_start: 
secs: 0 
nsecs: 0 

Le time_from_start est toujours 0. Donc, je pense que j'ai un problème dans mon code (ou dans mon code de lancement) mais je ne sais pas où.

Est-ce que quelqu'un a une idée de ce qui ne va pas? Je vous remercie.

Answer 1

J'ai résolu mon problème. Voici mon premier exemple que le travail et je l'expliquer après:

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

Si vous comparez les deux codes, j'initialisés (dans le premier) « traj.points.resize() » à 4. Il a été erreur parce que tous les points sont reliés les uns aux autres avec 1 parent ou 1 enfant. Donc, j'ai seulement 1 way-points (si j'ai 2 bras de robot, j'aurai eu 2 way-points ...) et ce way-points est défini par 4 positions (hanche, épaule, coude, poignet) . De plus, j'avais oublié d'initialiser et de redimensionner "traj.points [0] .positions" en 4 (nombres d'articulations). Enfin, "time_from_start = ros :: Duration (1)" n'a pas besoin d'être incrémenté, comme je l'ai lu, car c'est la vitesse du mouvement du bras du robot.