 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Controlling Pioneers using p2os (Python)
Description: This tutorial explains the basics of controlling a pioneer robot to wander by avoiding obstacles by reading sonar readings and operate gripper controls. This may not be the best code around, but it might be useful as a starting point.Keywords: p2os, pioneer, gripper, sonar
Tutorial Level: INTERMEDIATE
Contents
Installing and setting up p2os in pioneer
You may have to install a recent linux distribution in the computer in pioneer. Once that is done, you can setup ROS and p2os in that. Read Getting Started with p2os for instructions to setup p2os in the pioneer.
Launching p2os
Please read the documentation of p2os_driver for the list of topics published and subscribed and the list of parameters. p2os uses two parameters, namely "use_sonar" and "pulse". This can be set in the launch file while launching the p2os. You may edit the p2os_driver.launch file already existing in the p2os_launch folder or create a new launch file. In this tutorial a new launch file is created in p2os_launch folder.
1. Go to the p2os_launch folder by typing the following command
$ roscd p2os_launch
2. Create a launch file by typing the following. In this tutorial vim is assumed to be used. You may use any other text editor.
$ vim pioneer.launch
3. Enter the following text in the new launch file and save it.
<launch>
<group ns="pioneer">
<param name="pulse" value=1.0>
<param name="use_sonar" value=true>
<node pkg="p2os_driver" name="p2os" type="p2os">
</group>
</launch>4. You can launch the new launch file using the following.
$ roslaunch p2os_launch pioneer.launch
Create a pioneer class
Creating a package
Type the following command in the terminal to create a new package "pioneerControl" for our experiments. At the moment we are using only rospy, tf, nav_msgs and p2os_driver as the dependencies for this package.
$ roscreate pkg pioneerControl rospy tf nav_msgs p2os_driver
Type the following command so that ros start identifying your package
$ rospack profile
Now if you type the following you will be taken to the package directory.
$ roscd pioneerControl
You can check the dependencies are correctly set in the manifest.xml in the package folder.
If you face any problems in this step, please go through the beginner tutorials for better understanding of creating a new package.
Simple Model
In this a basic class of pioneer is created which can read the sonar, gripper status and odometry readings. We will create a node called rosPioneer for that.
$ mkdir nodes $ cd nodes $ vim rosPioneer1.py
Enter the following code in the newly created file.
1 #!/usr/bin/env python
2 # author: gauthampdas (pdasgautham-at-yahoo-dot-com)
3 # isrc, university of ulster, derry, uk
4
5 import roslib; roslib.load_manifest('pioneerControl')
6 import rospy
7 import nav_msgs.msg
8 import p2os_driver.msg
9 import geometry_msgs.msg
10
11 def processOdometry(self, odoMsg):
12 print "linear: x=%0.2f, y=%0.2f, z=%0.2f" %(odoMsg.pose.pose.position.x,odoMsg.pose.pose.position.y, odoMsg.pose.pose.position.z)
13 print "angular(quaternion): x=%0.2f, y=%0.2f, z=%0.2f, w=%0.2f" %(odoMsg.pose.pose.orientation.x,odoMsg.pose.pose.orientation.y, odoMsg.pose.pose.orientation.z, odoMsg.pose.pose.orientation.w)
14
15 def processSonar(self, sonMsg, sonLock):
16 print sonMsg.ranges
17
18 if __name__ == "__main__":
19 ns = "pioneer"
20 sonSub = rospy.Subscriber(ns+"/sonar", p2os_driver.msg.SonarArray, processSonar)
21 odoSub = rospy.Subscriber(ns+"/pose", nav_msgs.msg.Odometry, processOdometry)
22
23 ## publishers
24 velPub = rospy.Publisher(ns+"/cmd_vel", geometry_msgs.msg.Twist)
25 motPub = rospy.Publisher(ns+"/cmd_motor_state", p2os_driver.msg.MotorState)
26
27 rate = rospy.Rate(1)
28 count = 0
29 vel = geometry_msgs.msg.Twist()
30 vel.linear.x = 0.1
31 vel.angular.z = 0.0
32 while not rospy.is_shutdown():
33 velPub.publish(vel)
34 rate.sleep()
35 if count >20:
36 vel.linear.x = 0.0
37 vel.angular.z = 0.0
38 velPub.publish(vel)
39 break
40 count += 1
Running the new node
Make the newly created node executable.
$ chmod +x rosPioneer1.py
Type the following commands in the terminal to start running your node. Ensure that the space in front of the pioneer is clean for a smooth running. There are no obstacle avoidance incorporated. So the robot will hit any obstacle on the way.
$ rosrun pioneerControl rosPioneer1.py
Advanced Model
In this example we will implement a slightly advanced pioneer model.
$ vim rosPioneer2.py
Enter the following code in the newly created file.
1 #!/usr/bin/env python
2 # author: gauthampdas (pdasgautham-at-yahoo-dot-com)
3 # isrc, university of ulster, derry, uk
4
5 import roslib; roslib.load_manifest('pioneerControl')
6 import rospy
7 import nav_msgs.msg
8 import p2os_driver.msg
9 import tf
10 import geometry_msgs.msg
11
12 import multiprocessing
13 import time
14 import random
15
16 class pioneer():
17 def __init__(self,nameSpace):
18 ## using a custom name space
19 self.ns = nameSpace
20
21 ## subscribers
22 self.sonSub = rospy.Subscriber(self.ns+"/sonar", p2os_driver.msg.SonarArray, self.processSonar)
23 self.odoSub = rospy.Subscriber(self.ns+"/pose", nav_msgs.msg.Odometry, self.processOdometry)
24 self.griSub = rospy.Subscriber(self.ns+"/gripper_state", p2os_driver.msg.GripperState, self.processGripper)
25
26 ## publishers
27 self.velPub = rospy.Publisher(self.ns+"/cmd_vel", geometry_msgs.msg.Twist)
28 self.motPub = rospy.Publisher(self.ns+"/cmd_motor_state", p2os_driver.msg.MotorState)
29 self.griPub = rospy.Publisher(self.ns+"/gripper_control", p2os_driver.msg.GripperState)
30
31 ## data holders
32 self.pose = {"x":0.0, "y":0.0, "a":0.0}
33 self.sonData = [0.0 for i in range(16)]
34 # gripper has grip state and lift state
35 self.grip = {"state":0,"dir":0,"inner":False,"outer":False,"left":False,"outer":False}
36 self.lift = {"state":0, "dir"=0, "position":0.0}
37
38 def processOdometry(self, odoMsg):
39 """ > odomMsg = nav_msgs.msg.Odometry()
40 > get the position values and set them to self.pose"""
41 # set pose
42 self.pose["x"] = odoMsg.pose.pose.position.x
43 self.pose["y"] = odoMsg.pose.pose.position.y
44 theta = tf.transformations.euler_from_quaternion(odoMsg.pose.pose.orientation)
45 self.pose["a"] = theta[2]
46
47 def processSonar(self, sonMsg):
48 """ > sonMsg = p2os_driver.msg.SonarArray()
49 > get the range values and fill in sonData"""
50 if (sonMsg.ranges_count == 16):
51 # do the sonar data processing here
52 for sonIndex in range (0, 16): # read the sonar sensor data
53 self.sonData[sonIndex] = sonMsg.ranges[sonIndex]
54
55 def processGripper(self, griMsg):
56 self.grip["state"] = 0 + griMsg.grip.state
57 self.grip["dir"] = 0 + griMsg.grip.dir
58 self.grip["inner"] = True and griMsg.grip.inner_beam
59 self.grip["outer"] = True and griMsg.grip.outer_beam
60 self.grip["left"] = True and griMsg.grip.left_contact
61 self.grip["right"] = True and griMsg.grip.right_contact
62 self.lift["state"] = True and griMsg.lift.state
63 self.lift["dir"] = 0 + griMsg.lift.dir
64 self.lift["position"] = 0 + griMsg.lift.position
65
66 def gripperLower(self):
67 intToBool = lambda x: bool(x)
68 gripper = p2os_driver.msg.GripperState()
69 print "lower gripper"
70 if (self.lift["position"] > 0.1):
71 gripper.grip.state = 3 # status for no change
72 gripper.lift.dir = -1 # downward
73 gripper.lift.state = 5 # lower
74 self.griPub.publish(gripper)
75 time.sleep(5)
76 else:
77 print "reached bottom"
78
79 def gripperOpen(self):
80 intToBool = lambda x: bool(x)
81 gripper = p2os_driver.msg.GripperState()
82 print "open gripper"
83 if (self.grip["left"] or self.grip["right"]):
84 gripper.grip.state = 1 # open
85 gripper.grip.dir = 1 # outwards
86 gripper.lift.state = 6 # no change in lift
87 self.griPub.publish(gripper)
88 time.sleep(5)
89 else:
90 print "opened maximum"
91
92 def gripperClose(self):
93 intToBool = lambda x: bool(x)
94 gripper = p2os_driver.msg.GripperState()
95 print "close gripper"
96 if not(self.grip["left"]) or not(self.grip["right"]):
97 gripper.grip.state = 2 # close gripper
98 gripper.grip.dir = -1 # inwards
99 gripper.lift.state = 6 # no change in lift
100 self.griPub.publish(gripper)
101 time.sleep(5)
102 else:
103 print "closed maximum"
104
105 def gripperRaise(self):
106 intToBool = lambda x: bool(x)
107 gripper = p2os_driver.msg.GripperState()
108 print "raise gripper"
109 if (self.lift["position"] < 0.9):
110 gripper.grip.state = 3 # no change in grip
111 gripper.lift.dir = 1 # upward
112 gripper.lift.state = 4 # raise lift
113 self.griPublisher.publish(gripper)
114 time.sleep(5)
115 else:
116 print "raised maximum"
117
118 def pick(self):
119 self.gripperLower() # lowering
120 self.gripperOpen() # opening
121 self.gripperClose() # closing
122 self.gripperRaise() # raising
123
124 def stop(self):
125 vel = geometry_msgs.msg.Twist()
126 vel.linear.x = 0.0
127 vel.angular.z = 0.0
128 self.velPub.publish(vel)
129
130 def wander(self):
131 # wanders for some time
132 # uses odometry and sonar
133 vel = geometry_msgs.msg.Twist()
134 angle = {1:50.0, 2:30.0, 3:10.0, 4:-10.0, 5:-30.0, 6:-50.0}
135 timeNow = time.time()
136 while ((time.time() - timeNow) < 20): # run for 60 seconds
137 # find the sonar range with maximum range
138 # turn to that direction and move forward
139 vel.linear.x = 0.1
140 vel.angular.z = 0.0
141 maxRange = max(self.sonData[1:7])
142 if (maxRange > 2): # there is a direction with more than 2 meters open space
143 dir = self.sonData.index(maxRange)
144 if (dir == 3) or (dir == 4):
145 vel.linear.x = 0.1
146 vel.angular.z = 0.0
147 elif (dir < 3):
148 vel.linear.x = 0.0
149 vel.angular.z = 0.1
150 elif (dir > 3):
151 vel.linear.x = 0.0
152 vel.angular.z = -0.1
153 else: # turn back
154 vel.linear.x = -0.1
155 vel.angular.z = random.choice(0.1, -0.1)
156 self.velPub.publish(vel)
157 time.sleep(1)
158
159 if (__name__ == "__main__"):
160 rospy.init_node("rosPioneer2")
161 # creating an object of our robot class
162 myRobot = pioneer("pioneer")
163 # wander some distance and stop
164 myRobot.wander()
165 myRobot.stop()
166 time.sleep(2)
167 # simulate picking action
168 myRobot.pick()
169 time.sleep(2)
170 # wander some distance and stop
171 myRobot.wander()
172 myRobot.stop()
173 time.sleep(2)
174 # simulate picking action
175 myRobot.pick()
Save the file.
Running the new node
Make the newly created node executable.
$ chmod +x rosPioneer2.py
Type the following commands in the terminal to start running your node
$ rosrun pioneerControl rosPioneer2.py
Hope this is sufficient to start with the programming ros to control pioneer.
