Code and launch file update to account for namespaces

minesearch_project/launch/minefield.launch

@@ -13,9 +13,12 @@
   <arg name="loadMine5" default="1"/>
   <arg name="x5" default="2"/>
   <arg name="y5" default="40"/>
+  <arg name="namespace" default="robot1" />
 
   <include file="$(find minesearch_project)/launch/loadMine.xml" />
-  <include file="$(find pioneer_launch)/launch/upload_pioneer3at.xml" /> 
+  <include file="$(find pioneer_launch)/launch/upload_pioneer3at.launch">
+    <arg name="namespace" value="$(arg namespace)"/>
+  </include> 
 <!--  <include file="$(find cs4313_worlds)/launch/hokuyoGmapper.launch" /> -->
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
@@ -24,23 +27,38 @@
     <arg name="world_name" value="$(find minesearch_project)/worlds/minefield.world"/>
   </include>
 
-  <!-- launch the circle-finder node to detect and publish circular objects -->
-  <node name="circleFinder" pkg="minesearch_project" type="circleFinder.py" />  
-
   <!-- load mine-like objects into the scene based on argument values -->
-  <node name="spawn_mine1" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x1) -y $(arg y1) -urdf -param mine1 -model mine1" respawn="false" output="screen" />
-  <node name="spawn_mine2" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x2) -y $(arg y2) -urdf -param mine2 -model mine2" respawn="false" output="screen" />
+  <node name="spawn_mine1" pkg="gazebo_ros" type="spawn_model" 
+        args="-x $(arg x1) -y $(arg y1) -urdf -param mine1 -model mine1"
+        respawn="false" output="screen" />
+  <node name="spawn_mine2" pkg="gazebo_ros" type="spawn_model"
+        args="-x $(arg x2) -y $(arg y2) -urdf -param mine2 -model mine2"
+        respawn="false" output="screen" />
   <group if="$(arg loadMine3)">
-    <node name="spawn_mine3" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x3) -y $(arg y3) -urdf -param mine2 -model mine3" respawn="false" output="screen" />
+    <node name="spawn_mine3" pkg="gazebo_ros" type="spawn_model"
+          args="-x $(arg x3) -y $(arg y3) -urdf -param mine2 -model mine3"
+          respawn="false" output="screen" />
   </group>
   <group if="$(arg loadMine4)">
-    <node name="spawn_mine4" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x4) -y $(arg y4) -urdf -param mine2 -model mine4" respawn="false" output="screen" />
+    <node name="spawn_mine4" pkg="gazebo_ros" type="spawn_model"
+          args="-x $(arg x4) -y $(arg y4) -urdf -param mine2 -model mine4"
+          respawn="false" output="screen" />
   </group>  
   <group if="$(arg loadMine5)">
-    <node name="spawn_mine5" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x5) -y $(arg y5) -urdf -param mine2 -model mine5" respawn="false" output="screen" />
+    <node name="spawn_mine5" pkg="gazebo_ros" type="spawn_model"
+          args="-x $(arg x5) -y $(arg y5) -urdf -param mine2 -model mine5"
+          respawn="false" output="screen" />
   </group>  
 
+  <!-- launch the circle-finder node to detect and publish circular objects -->
+  <node name="robot1CircleFinder" pkg="minesearch_project" type="circleFinder.py" 
+        args="-n $(arg namespace)" />  
+
   <!-- push robot_description to factory and spawn robot in gazebo -->
-  <node name="spawn_pioneer" pkg="gazebo_ros" type="spawn_model" args="-z 0.0 -urdf -param robot_description -model robot_description" respawn="false" output="screen" />
+  <node name="spawn_pioneer" pkg="gazebo_ros" type="spawn_model"
+        args="-z 0.0 -urdf -param $(arg namespace)/p3at -model $(arg namespace)/p3at"
+        respawn="false" output="screen">
+    <remap from="robot_description" to="robot1/p3at"/>
+  </node>
 
 </launch>

minesearch_project/src/circleFinder.py

-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 import math
+import sys
 
 import rospy
 import sensor_msgs.msg as sensor_msgs
@@ -18,15 +19,17 @@ class CircleFinder:
     # @param nodeName: name of the circlefinder node
     # @param scanTopic: topic to which laser scan data is published
     # @param circleTopic: topic to which circle messages will be published
-    def __init__(self, nodeName="circleFinder", scanTopic="scan", \
-                 circleTopic="circles"):
-        rospy.init_node(nodeName)
+    def __init__(self, nodeName="CircleFinder", scanTopic="scan", \
+                 namespace="robot1", circleTopic="circles"):
+        rospy.init_node(namespace + nodeName)
         self.scan = src.LaserData()
         self.message = project_msgs.Circle()
         self.message.header.seq = 0
         self.message.header.frame_id = "laser"
-        self.publisher = rospy.Publisher(circleTopic, project_msgs.Circle, queue_size=1)
-        rospy.Subscriber(scanTopic, sensor_msgs.LaserScan, self.processLaser)
+        self.publisher = rospy.Publisher(namespace + "/" + circleTopic,
+                                         project_msgs.Circle, queue_size=1)
+        rospy.Subscriber(namespace + "/" + scanTopic, 
+                         sensor_msgs.LaserScan, self.processLaser)
 
 
     # Processes new laser data when it arrives.  Tests each interval of
@@ -45,7 +48,7 @@ class CircleFinder:
                 leftPt = (self.scan.ranges[interval[0]] * math.cos(leftAngle), \
                           self.scan.ranges[interval[0]] * math.sin(leftAngle))
 
-                middle = (interval[0] + interval[1]) / 2
+                middle = (interval[0] + interval[1]) // 2
                 middleAngle = self.scan.minAngle + middle * self.scan.increment
                 middleAngle = self.scan.rayBearing(middle)
                 middlePt = (self.scan.ranges[middle] * math.cos(middleAngle), \
@@ -56,13 +59,13 @@ class CircleFinder:
                 rightPt = (self.scan.ranges[interval[1]] * math.cos(rightAngle), \
                            self.scan.ranges[interval[1]] * math.sin(rightAngle))
 
-                leftTest = (interval[0] + middle) / 2
+                leftTest = (interval[0] + middle) // 2
                 leftTestAngle = self.scan.minAngle + leftTest * self.scan.increment
                 leftTestAngle = self.scan.rayBearing(leftTest)
                 leftTestPt = (self.scan.ranges[leftTest] * math.cos(leftTestAngle), \
                               self.scan.ranges[leftTest] * math.sin(leftTestAngle))
 
-                rightTest = (middle + interval[1]) / 2
+                rightTest = (middle + interval[1]) // 2
                 rightTestAngle = self.scan.minAngle + rightTest * self.scan.increment
                 rightTestAngle = self.scan.rayBearing(rightTest)
                 rightTestPt = (self.scan.ranges[rightTest] * math.cos(rightTestAngle), \
@@ -84,7 +87,14 @@ class CircleFinder:
 
 # Main Program (primarily for testing purposes for now)
 if __name__ == "__main__":
-    finder = CircleFinder()
+
+    ns = "robot1"
+    for argNum in range(len(sys.argv)-1):
+        if ((sys.argv[argNum] == '-n') or (sys.argv[argNum] == '--ns') or \
+            (sys.argv[argNum] == '--namespace')):
+            ns = sys.argv[argNum + 1]
+
+    finder = CircleFinder(namespace=ns)
     rospy.spin()