enabled range sensor layer

diff --git a/cfg/razor.yaml b/cfg/razor.yaml
index f32be3174918a4986dfd83204b92ddc94d4bc486..0f588317e926b02112b1ece2fe0d6f6014e1f271 100644 (file)
--- a/cfg/razor.yaml
+++ b/cfg/razor.yaml
@@ -23,8 +23,8 @@ magn_z_max: 596
 
 # extended calibration
 calibration_magn_use_extended: true
-magn_ellipsoid_center: [314.663, 115.689, -632.692]
-magn_ellipsoid_transform: [[0.937505, 0.0217526, 0.0102850], [0.0217526, 0.936649, 0.0279058], [0.0102850, 0.0279058, 0.980535]]
+magn_ellipsoid_center: [253.780, 56.6661, -712.749]
+magn_ellipsoid_transform: [[0.929568, 0.00880376, 0.0124014], [0.00880376, 0.949055, 0.00600124], [0.0124014, 0.00600124, 0.996672]]
 
 # AHRS to robot calibration
 imu_yaw_calibration: 0.0

diff --git a/config/base_local_planner_params.yaml b/config/base_local_planner_params.yaml
index 3b509c3ee603f5605462b5a1dbc5891e2b7069ce..eaff7f433397599401b61f7c46515acb27864491 100644 (file)
--- a/config/base_local_planner_params.yaml
+++ b/config/base_local_planner_params.yaml
@@ -11,3 +11,5 @@ TrajectoryPlannerROS:
 
   holonomic_robot: false
   meter_scoring: true
+
+  sim_granularity: 0.01

diff --git a/config/local_costmap_params.yaml b/config/local_costmap_params.yaml
index b4fa9847b9dc0c570fd4ad9a7e1d91a00381d78b..d4fd59ec96f68756b20bd7d9c1e3f1573f2fe9b5 100644 (file)
--- a/config/local_costmap_params.yaml
+++ b/config/local_costmap_params.yaml
@@ -5,14 +5,14 @@ local_costmap:
   publish_frequency: 2.0
   static_map: false
   rolling_window: true
-  width: 5.0
-  height: 5.0
+  width: 2.0
+  height: 2.0
   resolution: 0.05
   plugins:
   - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
-  #- {name: sonar_layer,   type: "range_sensor_layer::RangeSensorLayer"}
+  - {name: sonar_layer,   type: "range_sensor_layer::RangeSensorLayer"}
   - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
 
   sonar_layer:
-    topics: ["range_forward", "range_backward", "range_left", "range_right"]
+    topics: ["/range_forward", "/range_backward", "/range_left", "/range_right"]
     no_readings_timeout: 1.0

diff --git a/scripts/umbmark.py b/scripts/umbmark.py
index f031a675b9bf1e25855e27b16100509795a5dd96..398f2be10cde91fb16ff057e3ceb8b5634c49340 100755 (executable)
--- a/scripts/umbmark.py
+++ b/scripts/umbmark.py
@@ -40,6 +40,8 @@ from math import *
 from nav_msgs.msg import Odometry
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
 from actionlib_msgs.msg import GoalStatus
+from optparse import OptionParser
+
 
 class UMBMark:
        def __init__(self):
@@ -78,25 +80,29 @@ class UMBMark:
                        rospy.logerr("The base failed to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
                        raise
 
-       def run(self, direction=-1):
+       def run(self, direction=-1, size=2):
                while self.odom_pose is None:
                        sleep(0.1)
                init_pose = self.odom_pose
                for i in range(4):
-                       self.next_pos(2, 0, 0)
+                       self.next_pos(size, 0, 0)
                        self.next_pos(0, 0, direction*90*pi/180)
                final_pose = map(operator.sub, self.odom_pose, init_pose)
                print "Odom Pose: x=%.3f, y=%.3f, angle=%.3f°" % (final_pose[0], final_pose[1], final_pose[2]*180/pi)
 
-       def run_cw(self):
-               self.run(-1)
+       def run_cw(self, size):
+               self.run(-1, size)
 
-       def run_ccw(self):
-               self.run(1)
+       def run_ccw(self, size):
+               self.run(1, size)
 
 if __name__ == "__main__":
+       parser = OptionParser()
+       parser.add_option("-l", "--length", dest="length", default=2, help="Square size")
+       (options, args) = parser.parse_args()
+
        p = UMBMark()
        if len(sys.argv) > 1 and sys.argv[1] == "ccw":
-               p.run_ccw()
+               p.run_ccw(float(options.length))
        else:
-               p.run_cw()
+               p.run_cw(float(options.length))

diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index eb9f8bd9bb138bf74c6ec771d7850b5fa77706c3..a2a09ba1854a79694a98e4d1afbba89478bfde56 100755 (executable)
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -231,12 +231,12 @@ class MoveBase:
        def get_dist_forward(self):
                if self.pub_range_fwd.get_num_connections() > 0:
                        dist = self.get_dist_srf(0x5)
-                       self.send_range(self.pub_range_fwd, "sonar_forward", Range.ULTRASOUND, dist, 0.04, 6, 60)
+                       self.send_range(self.pub_range_fwd, "sonar_forward", Range.ULTRASOUND, dist, 0.04, 6, 30)
 
        def get_dist_backward(self):
                if self.pub_range_bwd.get_num_connections() > 0:
                        dist = self.get_dist_srf(0x7)
-                       self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, 6, 60)
+                       self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, 6, 30)
                
 
 if __name__ == "__main__":

diff --git a/urdf/wild_thumper.urdf.xacro b/urdf/wild_thumper.urdf.xacro
index 733a51d2e0b0267ab60bdb75c834ad1e2920d4c3..417123f89c611417a903a86f30c8fb4f76df3ff3 100644 (file)
--- a/urdf/wild_thumper.urdf.xacro
+++ b/urdf/wild_thumper.urdf.xacro
@@ -246,8 +246,8 @@
                </gazebo>
        </xacro:macro>
 
-       <xacro:range_sensor name="sonar_forward" ros_topic="range_forward" update_rate="10" minRange="0.04" maxRange="6" fov="${60*PI/180}" radiation="ultrasound" />
-       <xacro:range_sensor name="sonar_backward" ros_topic="range_backward" update_rate="10" minRange="0.04" maxRange="6" fov="${60*PI/180}" radiation="ultrasound" />
+       <xacro:range_sensor name="sonar_forward" ros_topic="range_forward" update_rate="10" minRange="0.04" maxRange="6" fov="${30*PI/180}" radiation="ultrasound" />
+       <xacro:range_sensor name="sonar_backward" ros_topic="range_backward" update_rate="10" minRange="0.04" maxRange="6" fov="${30*PI/180}" radiation="ultrasound" />
        <xacro:range_sensor name="ir_left" ros_topic="range_left" update_rate="10" minRange="0.04" maxRange="0.3" fov="${5*PI/180}" radiation="infrared" />
        <xacro:range_sensor name="ir_right" ros_topic="range_right" update_rate="10" minRange="0.04" maxRange="0.3" fov="${5*PI/180}" radiation="infrared" />
 </robot>