How to send data from child script to ros node

Typically: "How do I... ", "How can I... " questions
Post Reply
ljklonepiece
Posts: 105
Joined: 10 Oct 2013, 14:51

How to send data from child script to ros node

Post by ljklonepiece » 28 Feb 2014, 02:21

Hi Marc,

"Is there a way to send data generated in child script to the outside ros node?
specifically, I want motion planning module in vrep to generate a trajectory in the joint space for the robot arm and send them to ros node, after which the ros node will send command to real robot to execute such a trajectory.

I really appreciate if you can provide details on how to do this!
"

I guess i find the solution to the above problem and want to check with you!

Code: Select all

for i=1,6,1 do
	jh[i]=simGetObjectHandle('Mico_joint'..i)
	simExtROS_enablePublisher('/joint'..i,1,simros_strmcmd_get_joint_state,jh[i],-1,'')
end
I can see the topic published from the terminal in linux by issuing rotopic echo /joint1 etc

So I suppose this is right?

Code: Select all

name: ['Mico_joint1']
position: [-2.430983781814575]
velocity: [0.017766952514648438]
effort: [0.0]
Is the position here the joint angle of the joint1 in angular unit?
Thanks a lot

Yours sincerely,
Juekun

coppelia
Site Admin
Posts: 7682
Joined: 14 Dec 2012, 00:25

Re: How to send data from child script to ros node

Post by coppelia » 28 Feb 2014, 10:35

Hello Juekun,

yes, that is correct. But in order to send a motion sequence, I would take a different approach.
I would start by having a sequence of all joint positions of your robot. For a 3DoF robot, with 3 motion configurations (i.e. 3 nodes):

initialize a ROS publisher:

Code: Select all

-- in the initialization phase of your child script:
simExtROS_enablePublisher('motionData',99,simros_strmcmd_get_and_clear_string_signal,-1,-1,'myMotionData')
when you want to send motion data:

Code: Select all

path={0,0,0,0.1,0.05,0.2,0.2,0.1,0.4} -- 3 motion nodes (for a 3DoF manipulator)
packedData=simPackFloats(path)
oldData=simGetStringSignal('myMotionData')
if (oldData==nil) then
    oldData=''
end
simSetStringSignal('myMotionData',oldData..packedData)
In above code, if a motion sequence wasn't sent yet, it will be appended to the previous one. You might need to code additional data in order to distinguish individual motion sequences.

Then, in a ROS node, when you receive the string data, you can unpack it in a similar way:

Code: Select all

std_msgs::String theString;
float firstValue=((float*)(&theString[0]))[0];
float secondValue=((float*)(&theString[0]))[1];
...
above code is not tested, but should give you a rough idea how to do it.

Cheers

ljklonepiece
Posts: 105
Joined: 10 Oct 2013, 14:51

Re: How to send data from child script to ros node

Post by ljklonepiece » 01 Mar 2014, 07:04

Dear Marc,

Thanks for your advice!!!

Not sure i got what you mean:

Do you intend to group all the points on the trajectory into one data package and then publish the data package instead of sending each configuration on the trajectory one at a time?

say a 3-joints robot arm want to move end-effector from A to B, the trajectory generated by motion planning module return a trajectory containing 50 points, each point contains the configuration (joint angles) of 3 joints, say p[0]={1.3,2.4,0.3}, (suppose we control real robot by issuing joint angle command)
then all the points are concatenated to form data_package=p[0]..p[1].....p[48]..p[49]

the whole data_package is then published for ros node to subscribe and by breaking down the string received we can get the trajectory and apply them to real robot.

Does my interpretation make sense?
If so, what is the advantage of your approach compared with mine?
Thanks a lot

Yours sincerely,
juekun

coppelia
Site Admin
Posts: 7682
Joined: 14 Dec 2012, 00:25

Re: How to send data from child script to ros node

Post by coppelia » 01 Mar 2014, 23:12

Juekun,

you are right, and your method works too. But by sending the whole movement sequence at once, you have more safety and can be sure the data is consistent (and can be executed at regular time steps on the hardware).

We have now also improved the ROS plugin which was extended in following way:
  • there is an additional integer argument for simExtROS_enablePublisher: it allows to specify how often something should be published before going to sleep. 0 is for unlimited number of posts. -1 is when you want to start the publisher in sleep mode.
  • there is an additional command: simExtROS_wakePublisher(topicName,publishCnt): it allows to wake a publisher that is asleep, or to put a publisher asleep: -2 --> query the current publish cnt, -1 --> put the publisher to sleep, 0 --> wake the publisher with an unlimited number of posts, n --> wake the publisher for exactly n posts.
above modifications will be available in next release (V3.1.1), but you can already download the new plugin and source here.

This will allow to do following:
  • enable a publisher for string signals (e.g. simros_strmcmd_get_string_signal ) in sleeping mode. Do this just once, at simulation start.
  • When you have data to send, set it into a string signal, then wake the publisher for exactly one publishing. After one publishing, the publisher will go to sleep again.
  • Repeate above step every time you have something to send.
Cheers

Post Reply