From: Erik Andresen <erik@vontaene.de>
Date: Sun, 27 Sep 2015 10:10:07 +0000 (+0200)
Subject: enabled range sensor layer
X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=3708a6b10b4b3b98e1c3422f545dba82948fec5a;p=ros_wild_thumper.git
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 @@
</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>