enabled range sensor layer
authorErik Andresen <erik@vontaene.de>
Sun, 27 Sep 2015 10:10:07 +0000 (12:10 +0200)
committerErik Andresen <erik@vontaene.de>
Sun, 27 Sep 2015 10:10:07 +0000 (12:10 +0200)
cfg/razor.yaml
config/base_local_planner_params.yaml
config/local_costmap_params.yaml
scripts/umbmark.py
scripts/wt_node.py
urdf/wild_thumper.urdf.xacro

index f32be31..0f58831 100644 (file)
@@ -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
index 3b509c3..eaff7f4 100644 (file)
@@ -11,3 +11,5 @@ TrajectoryPlannerROS:
 
   holonomic_robot: false
   meter_scoring: true
+
+  sim_granularity: 0.01
index b4fa984..d4fd59e 100644 (file)
@@ -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
index f031a67..398f2be 100755 (executable)
@@ -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))
index eb9f8bd..a2a09ba 100755 (executable)
@@ -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__":
index 733a51d..417123f 100644 (file)
                </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>