From 53c52f19f4cd243c9f5a66780ca365d54820761f Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Sun, 11 Oct 2015 11:21:22 +0200 Subject: [PATCH] -navigation stack fixes -added cmd_vel_mux -robot_state_publisher testing -srf sensor timing fixes -added square.py --- cfg/cmd_vel_mux.yaml | 16 ++++ config/base_local_planner_params.yaml | 2 +- config/costmap_common_params.yaml | 2 +- config/local_costmap_params.yaml | 5 +- launch/move_base.launch | 1 + launch/robot_localization.launch | 124 ++++++++++++++++++++++++++ launch/teleop.launch | 1 - launch/wild_thumper.launch | 21 +++-- scripts/square.py | 56 ++++++++++++ scripts/wt_node.py | 25 ++++-- 10 files changed, 235 insertions(+), 18 deletions(-) create mode 100644 cfg/cmd_vel_mux.yaml create mode 100644 launch/robot_localization.launch create mode 100755 scripts/square.py diff --git a/cfg/cmd_vel_mux.yaml b/cfg/cmd_vel_mux.yaml new file mode 100644 index 0000000..6ed42cb --- /dev/null +++ b/cfg/cmd_vel_mux.yaml @@ -0,0 +1,16 @@ +subscribers: + - name: "Default input" + topic: "/cmd_vel" + timeout: 0.1 + priority: 0 + short_desc: "The default cmd_vel, controllers unaware that we are multiplexing cmd_vel should come here" + - name: "Navigation stack" + topic: "/move_base/cmd_vel" + timeout: 0.5 + priority: 1 + short_desc: "Navigation stack controller" + - name: "Wii Teleop" + topic: "/turtlebot_teleop_joystick/cmd_vel" + timeout: 0.1 + priority: 10 +publisher: "/cmd_vel_out" diff --git a/config/base_local_planner_params.yaml b/config/base_local_planner_params.yaml index eaff7f4..212e2b6 100644 --- a/config/base_local_planner_params.yaml +++ b/config/base_local_planner_params.yaml @@ -1,5 +1,5 @@ TrajectoryPlannerROS: - min_vel_x: 0.1 + min_vel_x: 0.2 max_vel_x: 0.3 min_vel_theta: -0.4 max_vel_theta: 0.4 diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index 2ddecc4..6e4b370 100644 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -1,5 +1,5 @@ footprint: [[0.14, 0.16], [-0.14, 0.16], [-0.14, -0.16], [0.14, -0.16]] -inflation_radius: 0.55 +inflation_radius: 0.5 obstacle_layer: observation_sources: laser_scan_sensor diff --git a/config/local_costmap_params.yaml b/config/local_costmap_params.yaml index d4fd59e..391e338 100644 --- a/config/local_costmap_params.yaml +++ b/config/local_costmap_params.yaml @@ -10,9 +10,10 @@ local_costmap: resolution: 0.05 plugins: - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'} - - {name: sonar_layer, type: "range_sensor_layer::RangeSensorLayer"} + - {name: range_layer, type: "range_sensor_layer::RangeSensorLayer"} - {name: inflation_layer, type: 'costmap_2d::InflationLayer'} - sonar_layer: + range_layer: topics: ["/range_forward", "/range_backward", "/range_left", "/range_right"] no_readings_timeout: 1.0 + clear_on_max_reading: true diff --git a/launch/move_base.launch b/launch/move_base.launch index ad30795..989cedc 100644 --- a/launch/move_base.launch +++ b/launch/move_base.launch @@ -33,5 +33,6 @@ + diff --git a/launch/robot_localization.launch b/launch/robot_localization.launch new file mode 100644 index 0000000..683c021 --- /dev/null +++ b/launch/robot_localization.launch @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [true, true, false, false, false, true, true, true, false, false, false, true, true, true, false] + [false, false, false, true, true, true, false, false, false, true, true, true, true, true, true] + [true, true, true, false, false, false, true, true, true, false, false, false, true, true, true] + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/teleop.launch b/launch/teleop.launch index 5fb511c..18f083c 100644 --- a/launch/teleop.launch +++ b/launch/teleop.launch @@ -6,7 +6,6 @@ - diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch index 869f51f..567d95a 100644 --- a/launch/wild_thumper.launch +++ b/launch/wild_thumper.launch @@ -2,9 +2,13 @@ + + - + + + @@ -24,14 +28,15 @@ - - - + + + + - + @@ -39,4 +44,10 @@ + + + + + + diff --git a/scripts/square.py b/scripts/square.py new file mode 100755 index 0000000..cd73274 --- /dev/null +++ b/scripts/square.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python +# -*- coding: iso-8859-15 -*- + +import rospy +import tf +import actionlib +from math import * +from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal +from actionlib_msgs.msg import GoalStatus +from optparse import OptionParser + + +class Square: + def __init__(self): + rospy.init_node('umbmark') + self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) + while not self.move_base.wait_for_server(rospy.Duration(5)): + rospy.loginfo("Waiting for the move_base action server to come up") + + def next_pos(self, x, y, angle): + odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle) + + rospy.loginfo("Moving to (%.2f, %2f), %d°.." % (x, y, angle*180/pi)) + + goal = MoveBaseGoal() + goal.target_pose.header.frame_id = "odom" + goal.target_pose.header.stamp = rospy.Time.now() + goal.target_pose.pose.position.x = x + goal.target_pose.pose.position.y = y + goal.target_pose.pose.orientation.x = odom_quat[0] + goal.target_pose.pose.orientation.y = odom_quat[1] + goal.target_pose.pose.orientation.z = odom_quat[2] + goal.target_pose.pose.orientation.w = odom_quat[3] + self.move_base.send_goal(goal) + + self.move_base.wait_for_result() + + if self.move_base.get_state() == GoalStatus.SUCCEEDED: + rospy.loginfo("The base moved to (%.2f, %2f), %d°" % (x, y, angle*180/pi)) + else: + rospy.logerr("The base failed to (%.2f, %2f), %d°" % (x, y, angle*180/pi)) + raise + + def run(self, size=1): + self.next_pos(0, 0, 0) + self.next_pos(size, 0, 0) + self.next_pos(size, -size, 0) + self.next_pos(0, size, 0) + +if __name__ == "__main__": + parser = OptionParser() + parser.add_option("-l", "--length", dest="length", default=2, help="Square size") + (options, args) = parser.parse_args() + + p = Square() + p.run(float(options.length)) diff --git a/scripts/wt_node.py b/scripts/wt_node.py index a2a09ba..4064fb8 100755 --- a/scripts/wt_node.py +++ b/scripts/wt_node.py @@ -4,6 +4,7 @@ import rospy import tf import struct +import prctl from i2c import * from math import * from geometry_msgs.msg import Twist @@ -16,7 +17,8 @@ WHEEL_DIST = 0.248 class MoveBase: def __init__(self): rospy.init_node('wild_thumper') - rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived) + prctl.set_name("wild_thumper") + rospy.Subscriber("cmd_vel_out", Twist, self.cmdVelReceived) rospy.Subscriber("imu", Imu, self.imuReceived) enable_odom_tf = rospy.get_param("~enable_odom_tf", True) if enable_odom_tf: @@ -47,8 +49,9 @@ class MoveBase: self.get_voltage() if i % 2: self.get_dist_forward() - self.get_dist_backward() self.get_dist_left() + else: + self.get_dist_backward() self.get_dist_right() i+=1 rate.sleep() @@ -193,13 +196,17 @@ class MoveBase: val = struct.unpack(">H", s)[0] return val - def get_dist_srf(self, num): + def start_dist_srf(self, num): dev = i2c(0x52) s = struct.pack("B", num) dev.write(s) dev.close() - sleep(50e-3) + def read_dist_srf(self, num): + dev = i2c(0x52) + s = struct.pack("B", num) + dev.write(s) + dev.close() dev = i2c(0x52) s = dev.read(2) @@ -221,22 +228,24 @@ class MoveBase: def get_dist_left(self): if self.pub_range_left.get_num_connections() > 0: dist = 30.553/(self.get_dist_ir(0x1) - -67.534) - self.send_range(self.pub_range_left, "ir_left", Range.INFRARED, dist, 0.04, 0.3, 5) + self.send_range(self.pub_range_left, "ir_left", Range.INFRARED, dist, 0.04, 0.3, 1) def get_dist_right(self): if self.pub_range_right.get_num_connections() > 0: dist = 17.4/(self.get_dist_ir(0x3) - 69) - self.send_range(self.pub_range_right, "ir_right", Range.INFRARED, dist, 0.04, 0.3, 5) + self.send_range(self.pub_range_right, "ir_right", Range.INFRARED, dist, 0.04, 0.3, 1) def get_dist_forward(self): if self.pub_range_fwd.get_num_connections() > 0: - dist = self.get_dist_srf(0x5) + dist = self.read_dist_srf(0x15) self.send_range(self.pub_range_fwd, "sonar_forward", Range.ULTRASOUND, dist, 0.04, 6, 30) + self.start_dist_srf(0x5) # get next value def get_dist_backward(self): if self.pub_range_bwd.get_num_connections() > 0: - dist = self.get_dist_srf(0x7) + dist = self.read_dist_srf(0x17) self.send_range(self.pub_range_bwd, "sonar_backward", Range.ULTRASOUND, dist, 0.04, 6, 30) + self.start_dist_srf(0x7) # get next value if __name__ == "__main__": -- 2.39.5