From 790aaf3a25c33986f1179102df98b7b71d7c33d5 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Tue, 17 Mar 2015 18:46:36 +0100 Subject: [PATCH] use new inflation_layer in layered costmap2d --- README | 1 - config/costmap_common_params.yaml | 4 -- config/explorer.rviz | 72 +++++++++++++----------------- config/global_costmap_params.yaml | 3 ++ config/local_costmap_params.yaml | 3 ++ config/map.png | Bin 2236 -> 0 bytes explorer_configuration.launch | 7 +-- launch/move_base.launch | 6 --- scripts/robo_explorer.py | 36 +++++---------- 9 files changed, 49 insertions(+), 83 deletions(-) delete mode 100644 config/map.png diff --git a/README b/README index c2fd482..b14225f 100644 --- a/README +++ b/README @@ -6,7 +6,6 @@ Its is split in the Nodes: Inputs are send via ft/get_inputs message. Outputs are set with ft/set_output and ft/set_motor message. -robo_explorer.py: Provides the Robo Explorer functions: - Laserscan is faked by the Sonar Sensor. You need to use the Robo Explorer with Wheels instead of Tracks because the Tracks are too inaccurate for Odometry. I used the Wheel Setup from Mobile Robots 2. diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index 1e07752..b4f0f06 100644 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -3,7 +3,3 @@ raytrace_range: 3.0 #footprint: [[x0, y0], [x1, y1], ... [xn, yn]] robot_radius: 0.105 inflation_radius: 0.15 - -observation_sources: laser_scan_sensor - -laser_scan_sensor: {sensor_frame: forward_sensor, data_type: LaserScan, topic: scan, marking: true, clearing: true} diff --git a/config/explorer.rviz b/config/explorer.rviz index e413e5f..6228578 100644 --- a/config/explorer.rviz +++ b/config/explorer.rviz @@ -5,7 +5,6 @@ Panels: Property Tree Widget: Expanded: - /TF1/Frames1 - - /Map1/Status1 - /Map1/Orientation1 Splitter Ratio: 0.618123 Tree Height: 656 @@ -27,7 +26,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: LaserScan + SyncSource: "" - Class: rviz/Tool Properties Expanded: ~ Name: Tool Properties @@ -57,35 +56,6 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 0 - Min Value: 0 - Value: true - Axis: Z - Channel Name: x - Class: rviz/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4.20249 - Min Color: 255; 255; 255 - Min Intensity: 3.74045 - Name: LaserScan - Position Transformer: XYZ - Queue Size: 16 - Selectable: true - Size (Pixels): 3 - Size (m): 0.01 - Style: Flat Squares - Topic: /scan - Use Fixed Frame: true - Use rainbow: true - Value: true - Alpha: 1 Class: rviz/Polygon Color: 25; 255; 0 @@ -98,7 +68,13 @@ Visualization Manager: Class: rviz/Path Color: 25; 255; 0 Enabled: true + Line Style: Lines + Line Width: 0.03 Name: Global Plan + Offset: + X: 0 + Y: 0 + Z: 0 Topic: /move_base/TrajectoryPlannerROS/global_plan Value: true - Alpha: 1 @@ -106,7 +82,13 @@ Visualization Manager: Class: rviz/Path Color: 0; 25; 255 Enabled: true + Line Style: Lines + Line Width: 0.03 Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 Topic: /move_base/TrajectoryPlannerROS/local_plan Value: true - Alpha: 1 @@ -114,7 +96,13 @@ Visualization Manager: Class: rviz/Path Color: 25; 255; 0 Enabled: true + Line Style: Lines + Line Width: 0.03 Name: Planner Plan + Offset: + X: 0 + Y: 0 + Z: 0 Topic: /move_base/NavfnROS/plan Value: true - Alpha: 1 @@ -195,11 +183,11 @@ Visualization Manager: Buffer Length: 1 Class: rviz/Range Color: 255; 255; 255 - Enabled: false + Enabled: true Name: Range Queue Size: 100 - Topic: /sonar_forward_lower - Value: false + Topic: /sonar + Value: true - Alpha: 0.7 Class: rviz/Map Color Scheme: costmap @@ -212,10 +200,10 @@ Visualization Manager: Class: rviz/Map Color Scheme: map Draw Behind: false - Enabled: true + Enabled: false Name: Map Topic: /map - Value: true + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 @@ -240,22 +228,22 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 3.63988 + Distance: 4.16269 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 3.49739 - Y: 1.95458 - Z: 0.198305 + X: 5.27226 + Y: 3.90177 + Z: 0.615206 Name: Current View Near Clip Distance: 0.01 - Pitch: 1.5598 + Pitch: 1.4798 Target Frame: Value: Orbit (rviz) - Yaw: 4.71126 + Yaw: 3.02126 Saved: ~ Window Geometry: Displays: diff --git a/config/global_costmap_params.yaml b/config/global_costmap_params.yaml index 3a20822..33c1f85 100644 --- a/config/global_costmap_params.yaml +++ b/config/global_costmap_params.yaml @@ -6,3 +6,6 @@ global_costmap: rolling_window: true width: 10.0 height: 10.0 + plugins: + - {name: sonar, type: "range_sensor_layer::RangeSensorLayer"} + - {name: inflation_layer, type: 'costmap_2d::InflationLayer'} diff --git a/config/local_costmap_params.yaml b/config/local_costmap_params.yaml index c8c35de..f3cfe7c 100644 --- a/config/local_costmap_params.yaml +++ b/config/local_costmap_params.yaml @@ -8,3 +8,6 @@ local_costmap: width: 4.0 height: 4.0 resolution: 0.01 + plugins: + - {name: sonar, type: "range_sensor_layer::RangeSensorLayer"} + - {name: inflation_layer, type: 'costmap_2d::InflationLayer'} diff --git a/config/map.png b/config/map.png deleted file mode 100644 index edbf44f271eb9fa26d3298689d8af289ef63e2f0..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2236 zcmeAS@N?(olHy`uVBq!ia0y~yV15C@96$kvj1%1!K#D2J+ueoXe|!I#{XiaPfk$L9 z0|U1(2s1Lwnj--eWH0gbb!ETDBP_-$Bb2z+oq>VF*VDx@q~g}w%Z$80-eH3S`=38% zj^gybrukskM1~$d30VWHgpva<8ZI&}W_Du-VPwIBFbS9-ifV`;nh`yGs75d@Zibi* zGXi88VRMmHgPlf%xkw&?X~%FcOaiBuvA7q*TnsNu$PzRc!^;O=5H=S*DCrhO2_>BH z5Q2GaG%P{!juH3haWn#wyW8~9q614-94$IPCE94w0j^UqO3R^MbhNJ*o8HTD-saeK R1z=l)!PC{xWt~$(699@P5ZC|! diff --git a/explorer_configuration.launch b/explorer_configuration.launch index 8b05a6d..4bff958 100644 --- a/explorer_configuration.launch +++ b/explorer_configuration.launch @@ -6,8 +6,6 @@ - - @@ -16,8 +14,5 @@ - - - - + diff --git a/launch/move_base.launch b/launch/move_base.launch index 378dd07..f78142f 100644 --- a/launch/move_base.launch +++ b/launch/move_base.launch @@ -1,11 +1,5 @@ - - - - - - diff --git a/scripts/robo_explorer.py b/scripts/robo_explorer.py index db7a227..bf35229 100755 --- a/scripts/robo_explorer.py +++ b/scripts/robo_explorer.py @@ -4,7 +4,7 @@ import rospy import tf from math import * from geometry_msgs.msg import Twist, TransformStamped, Point32, PoseWithCovarianceStamped -from sensor_msgs.msg import LaserScan +from sensor_msgs.msg import Range from nav_msgs.msg import Odometry from roboint.msg import Motor from roboint.msg import Inputs @@ -24,8 +24,6 @@ class RoboExplorer: self.y_last = 0 self.alpha_last = 0 - # fake laser scan with ultra sonic range finder - self.enable_ultrasonic_laser = bool(rospy.get_param('~ultrasonic_laser', "True")) # Distance between both wheels in meter (18.55cm) self.wheel_dist = float(rospy.get_param('~wheel_dist', "0.1855")) # Size of wheel Diameter in meter (5.15cm) * gear ratio (0.5) = 2.575cm @@ -36,8 +34,7 @@ class RoboExplorer: self.speed_constant = float(rospy.get_param('~speed_constant', "-1.7")) self.pub_motor = rospy.Publisher("ft/set_motor", Motor, queue_size=16) - if self.enable_ultrasonic_laser: - self.pub_scan = rospy.Publisher("scan", LaserScan, queue_size=16) + self.pub_sonar = rospy.Publisher("sonar", Range, queue_size=16) self.pub_odom = rospy.Publisher("odom", Odometry, queue_size=16) rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived) @@ -59,8 +56,7 @@ class RoboExplorer: self.update_odometry(msg, current_time) if (current_time - self.last_time).to_nsec() > 100e6: # send every 100ms self.send_odometry(msg, current_time) - if self.enable_ultrasonic_laser: - self.send_laser_scan(msg, current_time) + self.send_range(msg, current_time) self.last_time = current_time def update_odometry(self, msg, current_time): @@ -136,25 +132,17 @@ class RoboExplorer: # publish the message self.pub_odom.publish(odom) - def send_laser_scan(self, msg, current_time): - # actually ultra sonic range finder - num_points = 60 # The base planner needs at least 30 points to work in the default config - opening_angle = 30*pi/180 # each side - scan = LaserScan() + def send_range(self, msg, current_time): + # ultra sonic range finder + scan = Range() scan.header.stamp = current_time scan.header.frame_id = "forward_sensor" - scan.angle_min = -opening_angle - scan.angle_max = opening_angle - scan.angle_increment = (2*opening_angle)/num_points - scan.time_increment = 0.0 - scan.range_min = 0.0 - scan.range_max = 4.0 - for i in range(num_points): - scan.ranges.append(msg.d1/100.0) - #scan.intensities.append(0.5) - #scan.intensities.append(1.0) - #scan.intensities.append(0.5) - self.pub_scan.publish(scan) + scan.radiation_type = 0 + scan.field_of_view = 60*pi/180 + scan.min_range = 0.0 + scan.max_range = 4.0 + scan.range = msg.d1/100.0 + self.pub_sonar.publish(scan) # test with rostopic pub -1 cmd_vel geometry_msgs/Twist '[0, 0, 0]' '[0, 0, 0]' def cmdVelReceived(self, msg): -- 2.39.2