From: Erik Andresen Date: Sun, 27 Sep 2015 10:10:07 +0000 (+0200) Subject: enabled range sensor layer X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=commitdiff_plain;h=3708a6b10b4b3b98e1c3422f545dba82948fec5a;ds=sidebyside enabled range sensor layer --- diff --git a/cfg/razor.yaml b/cfg/razor.yaml index f32be31..0f58831 100644 --- 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 3b509c3..eaff7f4 100644 --- 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 b4fa984..d4fd59e 100644 --- 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 f031a67..398f2be 100755 --- 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 eb9f8bd..a2a09ba 100755 --- 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 733a51d..417123f 100644 --- a/urdf/wild_thumper.urdf.xacro +++ b/urdf/wild_thumper.urdf.xacro @@ -246,8 +246,8 @@ - - + +