I'm working on a robot arm, first, i wrote a physical model of the arm with a URDF files. This model work with Rviz and Gazebo. Moreover, i created a controllers.yaml file (for controls all robot's joints) and when I use the command :
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
both models (on rivz and gazebo) move simultaneously. But now, I want to create a .cpp file for the arm to move independently by using trajectory_msgs::JointTrajectory. Here's my cpp file :
#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;
}
When i launch my file.launch and I rosrun my cpp file, both are connected on rqt_graph. But immediately, i have an error (on launch terminal) :
[ERROR] [1497596211.214814221, 9.889000000]: Trajectory message contains waypoints that are not strictly increasing in time.
Effectively, when i use the command rostopic echo /arm_controller/command, i have :
positions: [0.0, 1.0, 0.0, 2.0]
velocities: []
accelerations: []
effort: []
time_from_start:
secs: 0
nsecs: 0
The time_from_start is always 0. So, I think i've a problem in my code (or in my launch code) but i don't know where.
Does anyone have an idea what is wrong? Thank you.
I resolved my problem. Here is my first example that working and i explain it after :
#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;
}
If you compare the two codes, I initialized (in the first one) "traj.points.resize()" to 4. It was a mistake because all points are connected to each other with 1 parent or 1 child. So, i've only 1 way-points (if i've 2 robot arm, i will have had 2 way-points...) and this way-points is defined by 4 positions (hip,shoulder,elbow,wrist). Moreover, i had forgotten to initialize and resize "traj.points[0].positions" to 4 (numbers of joints). Finally, "time_from_start = ros::Duration(1)" doesn't need to be incremented, as I read it, because it's the speed of the movement of the robot arm.