Add third sonar to ros config

diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml
index 998b0be7ab845f2edbf450984c8b9c9b364084e6..1f128f00d16f63c0b944ffb2d0478d8b3f3c9740 100644 (file)
--- a/config/costmap_common_params.yaml
+++ b/config/costmap_common_params.yaml
@@ -10,6 +10,6 @@ inflation_layer:
   inflation_radius: 0.20
 
 range_layer:
   inflation_radius: 0.20
 
 range_layer:

-  topics: ["/range_forward", "/range_backward", "/range_left", "/range_right"]
+  topics: ["/range_forward_left", "/range_backward", "/range_forward_right"]

   no_readings_timeout: 1.0
   clear_on_max_reading: true
   no_readings_timeout: 1.0
   clear_on_max_reading: true

diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index b4bb8ebbf1e4671e347faacd16c088aad9c613fd..50cde9f5af3b10b75281767562a0924bd14747c5 100755 (executable)
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -60,7 +60,8 @@ class MoveBase:
                self.dyn_conf = Server(WildThumperConfig, self.execute_dyn_reconf)
                self.pub_odom = rospy.Publisher("odom", Odometry, queue_size=16)
                self.pub_diag = rospy.Publisher("diagnostics", DiagnosticArray, queue_size=16)
                self.dyn_conf = Server(WildThumperConfig, self.execute_dyn_reconf)
                self.pub_odom = rospy.Publisher("odom", Odometry, queue_size=16)
                self.pub_diag = rospy.Publisher("diagnostics", DiagnosticArray, queue_size=16)

-               self.pub_range_fwd = rospy.Publisher("range_forward", Range, queue_size=16)
+               self.pub_range_fwd_left = rospy.Publisher("range_forward_left", Range, queue_size=16)
+               self.pub_range_fwd_right = rospy.Publisher("range_forward_right", Range, queue_size=16)

                self.pub_range_bwd = rospy.Publisher("range_backward", Range, queue_size=16)
                self.pub_range_left = rospy.Publisher("range_left", Range, queue_size=16)
                self.pub_range_right = rospy.Publisher("range_right", Range, queue_size=16)
                self.pub_range_bwd = rospy.Publisher("range_backward", Range, queue_size=16)
                self.pub_range_left = rospy.Publisher("range_left", Range, queue_size=16)
                self.pub_range_right = rospy.Publisher("range_right", Range, queue_size=16)


@@ -81,20 +82,32 @@ class MoveBase:
                sleep(3) # wait 3s for ros to register and establish all subscriber connections before sending reset diag
                reset_val = self.get_reset()
                rospy.loginfo("Reset Status: 0x%x" % reset_val)
                sleep(3) # wait 3s for ros to register and establish all subscriber connections before sending reset diag
                reset_val = self.get_reset()
                rospy.loginfo("Reset Status: 0x%x" % reset_val)

-               i = 0
+               ir_count = 0
+               sonar_count = 0

                while not rospy.is_shutdown():
                        rospy.logdebug("Loop alive")
                        #print struct.unpack(">B", i2c_read_reg(0x50, 0xA2, 1))[0] # count test
                        self.get_motor_err()
                        self.get_odom()
                        self.get_voltage()
                while not rospy.is_shutdown():
                        rospy.logdebug("Loop alive")
                        #print struct.unpack(">B", i2c_read_reg(0x50, 0xA2, 1))[0] # count test
                        self.get_motor_err()
                        self.get_odom()
                        self.get_voltage()

-                       if i % 2:
-                               self.get_dist_forward()
+
+                       if ir_count == 0:

                                self.get_dist_left()
                                self.get_dist_left()

+                               ir_count+=1

                        else:
                        else:

-                               self.get_dist_backward()

                                self.get_dist_right()
                                self.get_dist_right()

-                       i+=1
+                               ir_count+=1
+
+                       if sonar_count == 0:
+                               self.get_dist_forward_left()
+                               sonar_count=0
+                       elif sonar_count == 1:
+                               self.get_dist_backward()
+                               sonar_count+=1
+                       elif sonar_count == 2:
+                               self.get_dist_forward_right()
+                               sonar_count=0
+

                        if self.cmd_vel != None:
                                self.set_speed(self.cmd_vel[0], self.cmd_vel[1])
                                self.cur_vel = self.cmd_vel
                        if self.cmd_vel != None:
                                self.set_speed(self.cmd_vel[0], self.cmd_vel[1])
                                self.cur_vel = self.cmd_vel


@@ -296,10 +309,10 @@ class MoveBase:
                        if dist > 69:
                                self.send_range(self.pub_range_right, "ir_right", Range.INFRARED, 17.4/(dist - 69), 0.04, 0.3, 1)
 
                        if dist > 69:
                                self.send_range(self.pub_range_right, "ir_right", Range.INFRARED, 17.4/(dist - 69), 0.04, 0.3, 1)
 

-       def get_dist_forward(self):
-               if self.pub_range_fwd.get_num_connections() > 0:
+       def get_dist_forward_left(self):
+               if self.pub_range_fwd_left.get_num_connections() > 0:

                        dist = self.read_dist_srf(0x15)
                        dist = self.read_dist_srf(0x15)

-                       self.send_range(self.pub_range_fwd, "sonar_forward", Range.ULTRASOUND, dist, 0.04, self.range_sensor_max, 30)
+                       self.send_range(self.pub_range_fwd, "sonar_forward_left", Range.ULTRASOUND, dist, 0.04, self.range_sensor_max, 30)

                        self.start_dist_srf(0x5) # get next value
 
        def get_dist_backward(self):
                        self.start_dist_srf(0x5) # get next value
 
        def get_dist_backward(self):


@@ -307,6 +320,12 @@ class MoveBase:
                        dist = self.read_dist_srf(0x17)
                        self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, self.range_sensor_max, 30)
                        self.start_dist_srf(0x7) # get next value
                        dist = self.read_dist_srf(0x17)
                        self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, self.range_sensor_max, 30)
                        self.start_dist_srf(0x7) # get next value

+
+       def get_dist_forward_right(self):
+               if self.pub_range_fwd_right.get_num_connections() > 0:
+                       dist = self.read_dist_srf(0x19)
+                       self.send_range(self.pub_range_fwd, "sonar_forward_right", Range.ULTRASOUND, dist, 0.04, self.range_sensor_max, 30)
+                       self.start_dist_srf(0x9) # get next value

        
        def led_stripe_received(self, msg):
                for led in msg.leds:
        
        def led_stripe_received(self, msg):
                for led in msg.leds:

diff --git a/urdf/wild_thumper.urdf.xacro b/urdf/wild_thumper.urdf.xacro
index f84e201314ddb72307d1042b9f5cee7f38d5c7b5..3cc49220f73b9fc486f90258655404fa52d36c0a 100644 (file)
--- a/urdf/wild_thumper.urdf.xacro
+++ b/urdf/wild_thumper.urdf.xacro
@@ -56,7 +56,19 @@
                <origin xyz="0.107 0.0 0.04" rpy="0 0 0"/>
        </xacro:asus_camera>
 
                <origin xyz="0.107 0.0 0.04" rpy="0 0 0"/>
        </xacro:asus_camera>
 

-       <link name="sonar_forward">
+       <link name="sonar_forward_left">
+               <visual>
+                       <geometry>
+                               <box size="0.016 0.044 0.02"/>
+                       </geometry>
+                       <origin xyz="${0.016/2} 0 0" rpy="0 0 0"/>
+                       <material name="green">
+                               <color rgba="0 1 0 0.8"/>
+                       </material>
+               </visual>
+       </link>
+
+       <link name="sonar_forward_right">

                <visual>
                        <geometry>
                                <box size="0.016 0.044 0.02"/>
                <visual>
                        <geometry>
                                <box size="0.016 0.044 0.02"/>


@@ -122,10 +134,16 @@
                <origin xyz="0.06 -0.03 0.058" rpy="0 0 0"/>
        </joint>
 
                <origin xyz="0.06 -0.03 0.058" rpy="0 0 0"/>
        </joint>
 

-       <joint name="sonar_forward_joint" type="fixed">
+       <joint name="sonar_forward_left_joint" type="fixed">
+               <parent link="mounting_plate"/>
+               <child link="sonar_forward_left"/>
+               <origin xyz="0.115 -0.04 -0.012" rpy="0 0 0"/>
+       </joint>
+
+       <joint name="sonar_forward_right_joint" type="fixed">

                <parent link="mounting_plate"/>
                <parent link="mounting_plate"/>

-               <child link="sonar_forward"/>
-               <origin xyz="0.115 0.0 -0.012" rpy="0 0 0"/>
+               <child link="sonar_forward_right"/>
+               <origin xyz="0.115 0.04 -0.012" rpy="0 0 0"/>

        </joint>
 
        <joint name="sonar_backward_joint" type="fixed">
        </joint>
 
        <joint name="sonar_backward_joint" type="fixed">