(!) 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

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.

Wiki: p2os/Tutorials/Controlling Pioneers using p2os (last edited 2012-09-17 10:51:56 by GauthamDas)