roslibjs
roslibjs copied to clipboard
duration for joint trajectory msg
Hello,
I am trying to send trajectory_msg/JointTrajectory msg from rosjslib over ros bridge. On echoing the command topic for my robotic arm joint position trajectory controller, I can see the topics being published by web socket.
The problem is that rosbridge handles all the timings and JointTrajectoryPoint has a 'Time_From_Start' field (which is a Duration msg from std_msg/Duration) and it sets it automatically to 0. This lets the trajectory controller not work as it seems the time_from_start parameter should be greater than 0.
https://github.com/RobotWebTools/rosbridge_suite/blob/develop/rosbridge_library/src/rosbridge_library/internal/message_conversion.py#L250
It does not matter even if I change the time_from_start to some value before publishing, it always becomes 0.
Please suggest any fix?
I found a hack to get this problem solved. I subscribed to a custom topic with JointTrajectory msg, I published on this custom msg from JavaScript and when I received the trajectory msg, I changed the time from start duration and publish it to arm controller command topic.
However I do not like this hack very much but it kinda works.
@arsh09 can you please share the code on how to send trajectory_msg messages via rosjslib? I am also trying to send a trajectory_msg/JointTrajectory from rosjslib, but I did not manage yet to 'build' the write message in javascript. Can you please posted here how you define the function for trajectory_msg? and also how exactly are you changing the 'Time_From_Start' duration? Thanks!
@ccopot Sure. I have this function for sending the trajectory message to ROS master (I am using VueJS).
var trajectoryMsg = new ROSLIB.Message({
header : {
frame_id : payload.frame_id
},
joint_names : payload.joint_names,
points : [
{
positions : payload.joint_values, // array of positions e.g. [1,0,0.3,...]
velocities : [],
accelerations : [],
effort : [],
time_from_start : payload.time_from_start
}
]
});
// Publishing multiple times as one time publish sometimes get lost.
for (var j = 0; j < 10; j++){
trajectoryController.publish(trajectoryMsg)
}
},
As for solving the time_from_start, instead of sending the joint trajectory message directly to the topic, I wrote a node that receives this message, change the time_from_start from 0 to suitable value and then publish that 'editted' message to the topic. This is a cheap hack but that is what seems to work.
Another way is to use MoveIt commander but as it creates a plan, and then execute (even when the input are joint values), it is a little less 'responsive'
Hit the same issue, was not sure how to declare the time_from_start field directly in javascript.
If you define the fields of the type Duration for secs
and nsecs
it is properly parsed on ROS side as valid trajectory msg. Seems more clean than the proxy node hack.
trajectory: {
joint_names: [
"shoulder_pan_joint",
"shoulder_lift_joint",
"elbow_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"
],
points: [
{positions: [0, -1.57, 1.57, 0, 0, 0], time_from_start: {secs: 3, nsecs: 0}},
{positions: [-1.57, 0, -1.57, 0, 0, 0], time_from_start: {secs: 6, nsecs: 0}},
{positions: [0, 1.57, 1.57, 0, 0, 0], time_from_start: {secs: 9, nsecs: 0}}
]
}
It seems https://github.com/RobotWebTools/roslibjs/issues/358#issuecomment-977816966 solved the issue.