V-REP + ROS to run gmapping

Typically: "How do I... ", "How can I... " questions
Post Reply
erenes
Posts: 14
Joined: 16 Oct 2013, 09:36

V-REP + ROS to run gmapping

Post by erenes »

Hello,

I am trying to run gmapping on ROS using V-REP.

Q1. this is possible? (I think yes. That is why I try to do)

Q2. The following is what I've done so far. Is this correct to run gmapping using V-REP?

I use Pioneer_p3dx + Hokuyo_URG_04LX_UR01_ROS in V-REP.
This Hokuyo sensor model publish sensor_msgs/LaserScan. So this is thankful. I don't need to make it by myself.
In child script of Hokuyo sensor, I added tf for laser.

Code: Select all

	parentTf=simGetObjectHandle("base_link")  	
	tfname=simExtROS_enablePublisher('/base_scan',1,simros_strmcmd_get_transform ,modelHandle,parentTf,'')
'base_link' is name of Pioneer_p3dx in scene hierarchy. I changed name.

And I added this code in the child script of Pioneer_p3dx.

Code: Select all

   robotPioneer=simGetObjectHandle("base_link")
	tfname=simExtROS_enablePublisher('/base_link',1,simros_strmcmd_get_transform ,robotPioneer,-1,'') 
	rpName=simExtROS_enablePublisher('/odom',1,simros_strmcmd_get_odom_data,robotPioneer,-1,'') 
	cmdTopic=simExtROS_enableSubscriber('/cmd_vel',1,simros_strmcmd_set_twist_command,-1,-1,'twistSignal')
TF tree is like this. /world --> /base_link --> /base_scan
I think /world is decided by ROS plugin of V-REP.

After this I could see laser, tf, odometry in rviz of ROS.

If I am right, what can I do next?
or let me know what is wrong.

Thanks.

erenes
Posts: 14
Joined: 16 Oct 2013, 09:36

Re: V-REP + ROS to run gmapping

Post by erenes »

I looked up example of turtlebot.
They made TF tree like this.
/map --> /odom --> /base_footprint --> /base_link --> /base_scan

How can I make this TF tree when I use V-REP? (Pioneer_p3dx + Hokuyo)

What I undestood is...
/world : we don't need TF for world ? This may be needed only in V-REP ?
/map : this is TF for costmap coordinate. But don't know yet how to broadcast this.
/odom : this is TF for cooridinate in which odometry is calculated. But how can I make this TF in V-REP?
/base_footprint : I don't know about this. .. -_-;
/base_link : This is ceter of mass of robot. Am I right? Why we need this?
/base_scan : This is coordinate is which sensor data is grabbed.

Thank you

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

Re: V-REP + ROS to run gmapping

Post by coppelia »

Hello,

we unfortunately have no experience with gmapping..

Cheers

erenes
Posts: 14
Joined: 16 Oct 2013, 09:36

Re: V-REP + ROS to run gmapping

Post by erenes »

I see. Thank you anyway.

But I found the solution. So it works now. I have no blog. So can't link some images to let you guys understand what I've done.
But I will try to explain here to share. (Anyway ROS is quite complicated especially for me. very beginner -_-;)

Goal : (Pioneer_3dx + Hokuyo_URG_04LX_UR01_ROS) in V-REP <---> gmapping in ROS

Procedure :
0. Run V-REP
1. Make instance of Pioneer_3dx. And change name to 'base_link'
2. Make instance of Hokuyo_URG_04LX_UR01_ROS. And change name to 'base_scan'
3. Locate the laser sensor to a position which you like to put it on robot. And group it together.
4. So... we have robot.

5. Edit child script of 'base_scan' (hokuyo laser sensor)
We don't need two lines. So comment it. We will make static TF later for /base_link --> /base_scan.

Code: Select all

	topicName=simExtROS_enablePublisher('front_scan',1,simros_strmcmd_get_laser_scanner_data ,modelHandle,-1,'rangeDat'..objName)	

	--parentTf=simGetObjectHandle("parentTf#")  --get handle to parent object in tf tree. Change this to your needs
	--tfname=simExtROS_enablePublisher('tf',1,simros_strmcmd_get_transform ,modelHandle,parentTf,'') --publish the tf
6. Edit chid script of 'base_link' (pioneer-3dx robot)

Code: Select all

	braitenbergL={-0.2,-0.4,-0.6,-0.8,-1,-1.2,-1.4,-1.6, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}
	braitenbergR={-1.6,-1.4,-1.2,-1,-0.8,-0.6,-0.4,-0.2, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}
	v0=2
	
   -- Added from here --------------------------------------------------------------------------
	-- Stop motor for initialization
	simSetJointTargetVelocity(motorLeft,0.0)
	simSetJointTargetVelocity(motorRight,0.0)

	robotPioneer=simGetObjectHandle('base_link')

	rpName=simExtROS_enablePublisher('/odom',1,simros_strmcmd_get_odom_data,robotPioneer,-1,'') 
	tfname=simExtROS_enablePublisher('tf',1,simros_strmcmd_get_transform ,robotPioneer,-1,'base_link%odom') 
	cmdTopic=simExtROS_enableSubscriber('/turtle1/cmd_vel',1,simros_strmcmd_set_twist_command,-1,-1,'twistSignal')
   -- Added to here ------------------------------------------------------------------------------

end 

if (sim_call_type==sim_childscriptcall_cleanup) then 

Code: Select all

	for i=1,16,1 do
		vLeft=vLeft+braitenbergL[i]*detect[i]
		vRight=vRight+braitenbergR[i]*detect[i]
	end
  
    -- Added from here -----------------------------------------------------
	vLeft = 0.0
	vRight = 0.0
	
	packedData=simGetStringSignal('twistSignal')			
	if(packedData) then
		twistData=simUnpackFloats(packedData)
		-- Do something with the twist data
		--print(twistData[1], twistData[2], twistData[3], twistData[4], twistData[5], twistData[6])
		linearx = twistData[1]
		angularz = twistData[6]
		robotBase = 0.315
		vLeft = linearx - (robotBase / 2)*angularz
		vRight = linearx + (robotBase / 2)*angularz
		print(vLeft, vRight)
	end
   -- Added to here ----------------------------------------------------------------------

	simSetJointTargetVelocity(motorLeft,vLeft)
	simSetJointTargetVelocity(motorRight,vRight)
7. Now robot is ready in V-REP. Save this scene. Close V-REP.

8. Open a terminal. And run roscore
9. Run V-REP again.
10. Open the scene that we just made.
11. Put some obstacles around the robot.
12. Open new terminal and run this.
linux:~$ rosrun tf static_transform_publisher 0.2 0.0 0.1 0.0 0.0 0.0 base_link base_scan 50

13. Run rviz
14. Open new terminal and run this.
linux:~$ rosrun gmapping slam_gmapping scan:=base_scan

15. Start simulation in V-REP
16. Add map in rviz
17. Open new terminal and run this.
linux:~$ rosrun turtlesim turtle_teleop_key

18. Press arrow key on keyboard. Robot should move by arrow key.
19. You can see grid map in rviz.

20. Move your robot all the place in environment. And make sure map is completed.
21. Open new terminal and run this.
linux:~$ rosrun map-server map-saver

22. You can see map.pgm file and map.yaml file. This map file will be used in navigation.

If some reader want explanation about navigation after this, I will explain it here.
Cheers.

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

Re: V-REP + ROS to run gmapping

Post by coppelia »

Thanks a lot for the clear and detailed instructions!!

Cheers

skywalker
Posts: 3
Joined: 01 Aug 2015, 04:48

Re: V-REP + ROS to run gmapping

Post by skywalker »

This is exactly what I am trying to do.
Thank you very much.

skywalker
Posts: 3
Joined: 01 Aug 2015, 04:48

Re: V-REP + ROS to run gmapping

Post by skywalker »

erenes:

When I make gmapping subscribe base_scan, it did not create map. Because VREP was still sensding laser data to /vrep/front_scan topic.

You need to change this line.

Code: Select all

topicName=simExtROS_enablePublisher('front_scan',1,simros_strmcmd_get_laser_scanner_data ,modelHandle,-1,'rangeDat'..objName)
To create map I run like this:

Code: Select all

rosrun gmapping slam_gmapping scan:=/vrep/front_scan

aaron_cheng
Posts: 7
Joined: 11 Nov 2015, 17:37

Re: V-REP + ROS to run gmapping

Post by aaron_cheng »

Thank you, Erenes.

There is still a small error in step 21. The correct command line should be like this:
linux:~$ rosrun map_server map_saver

Post Reply