Hi there. I'm currently trying to use move_base, base_local_planner and frontier_exploration on a turtlebot 2 to autonomously explore a room. I currently am using a static map (from hector mapping) for the global costmap and building an obstacle layer in my local costmap as I go with my horizontal hokuyo. The config files that I am using are based off the husky_navigation config files.
These are my parameter files for the costmaps (I can add the rest of my config files for the planners etc if they're needed):
costmap_common.yaml
#footprint: [[-0.5, -0.33], [-0.5, 0.33], [0.5, 0.33], [0.5, -0.33]]
robot_radius: 0.25
footprint_padding: 0.01
robot_base_frame: base_footprint
update_frequency: 4.0
publish_frequency: 3.0
transform_tolerance: 0.5
resolution: 0.05
obstacle_range: 5.5
raytrace_range: 6.0
#layer definitions
static:
map_topic: /map
subscribe_to_updates: true
obstacles_laser:
observation_sources: laser
laser: {data_type: LaserScan, sensor_frame: /laser_lsl, clearing: true, marking: true,
topic: /scan_lsl, inf_is_valid: true}
inflation:
inflation_radius: 1.0
costmap_local.yaml
global_frame: odom
rolling_window: true
update_frequency: 5.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 20.0
height: 20.0
resolution: 0.05
transform_tolerance: 0.5
plugins:
- {name: obstacles_laser, type: "costmap_2d::ObstacleLayer"}
- {name: inflation, type: "costmap_2d::InflationLayer"}
costmap_global.yaml
global_frame: map
static_map: true
rolling_window: false
track_unknown_space: true
update_frequency: 1.0
publish_frequency: 0.5
transform_tolerance: 0.5
plugins:
- {name: static, type: "costmap_2d::StaticLayer"}
- {name: inflation, type: "costmap_2d::InflationLayer"}
I am able to get the correct robot pose using hector mapping and can also get the correct static map, global costmap and local costmap. Images here
The issue is that when I try and send the robot movement commands using either a 2D nav goal or set a region to be explored, the robot doesn't move as it thinks it's stuck. It prints:
[ WARN] [1465787788.724254866]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1465787799.324231262]: Rotate recovery behavior started.
I've run the program in debugging mode and this is what it prints for the various nodes that are running (note. I've deleted some of the repeated lines to keep the output more readable):
Local planner
[DEBUG] [1465797982.956315651]: Path/Goal distance computed
[DEBUG] [1465797982.958396979]: Trajectories created
[DEBUG] [1465797982.987519030]: Cost PointCloud published
[DEBUG] [1465797982.987591393]: A valid velocity command of (0.10, 0.00, -0.22) was found for this cycle.
[DEBUG] [1465797983.141464666]: Nearest waypoint to <0.000000, 0.000000> is <-0.025508, -0.019038>
costmap_2d
[DEBUG] [1465798020.176874025]: Updating area x: [293, 485] y: [174, 365]
[DEBUG] [1465798020.180376176]: Map update time: 0.004092932
[DEBUG] [1465798020.203780619]: MessageFilter [target=odom laser_lsl ]: Message ready in frame laser_lsl at time 1465798020.106, count now 3
[DEBUG] [1465798020.203981715]: MessageFilter [target=odom laser_lsl ]: Message ready in frame laser_lsl at time 1465798020.131, count now 2
[DEBUG] [1465798020.215840402]: MessageFilter [target=odom laser_lsl ]: Added message in frame laser_lsl at time 1465798020.181, count now 2
[DEBUG] [1465798020.217410425]: Received a 1024 X 1024 map at 0.050000 m/pix
[DEBUG] [1465798020.224168259]: MessageFilter [target=odom laser_lsl ]: Added message in frame laser_lsl at time 1465798020.206, count now 3
[DEBUG] [1465798020.253480212]: MessageFilter [target=odom laser_lsl ]: Message ready in frame laser_lsl at time 1465798020.156, count now 3
[DEBUG] [1465798020.253653054]: MessageFilter [target=odom laser_lsl ]: Message ready in frame laser_lsl at time 1465798020.181, count now 2
[DEBUG] [1465798020.263717589]: MessageFilter [target=odom laser_lsl ]: Added message in frame laser_lsl at time 1465798020.232, count now 2
[DEBUG] [1465798020.303749904]: MessageFilter [target=odom laser_lsl ]: Message ready in frame laser_lsl at time 1465798020.206, count now 2
[DEBUG] [1465798020.303949480]: MessageFilter [target=odom laser_lsl ]: Message ready in frame laser_lsl at time 1465798020.232, count now 1
move_base
[DEBUG] [1465798058.148772941]: Publishing feedback for goal, id: /move_base-1-1465798053.748505143, stamp: 1465798053.75
[DEBUG] [1465798058.148839593]: Publishing feedback for goal with id: /move_base-1-1465798053.748505143 and stamp: 1465798053.75
[DEBUG] [1465798058.148888789]: In controlling state.
[DEBUG] [1465798058.258524863]: Got a valid command from the local planner: 0.100, 0.000, -0.225
[DEBUG] [1465798058.258590229]: Full control cycle time: 0.109871963
navfn
[DEBUG] [1465798122.176390777]: [Path] 0.88,0.47 -0.68,0.73 0.85,0.53 -0.77,0.64; final x=0.983, y=0.489
[DEBUG] [1465798122.176457587]: [Path] 0.85,0.53 -0.77,0.64 0.80,0.61 -0.86,0.52; final x=0.004, y=0.568
[DEBUG] [1465798122.176479728]: [Path] 0.80,0.61 -0.86,0.52 0.98,-0.18 -0.98,-0.22; final x=-0.015, y=0.404
roscpp
[DEBUG] [1465798160.975194536]: Timer deregistering callbacks.
[DEBUG] [1465798161.003706278]: Incoming queue full for topic "/odom". Discarding oldest message (current queue size [0])
[DEBUG] [1465798161.043929531]: Incoming queue full for topic "/odom". Discarding oldest message (current queue size [0])
[DEBUG] [1465798161.763649884]: Incoming queue full for topic "/odom". Discarding oldest message (current queue size [0])
[DEBUG] [1465798161.803722786]: Incoming queue full for topic "/odom". Discarding oldest message (current queue size [0])
[DEBUG] [1465798161.831200829]: Accepted connection on socket [6], new socket [25]
[DEBUG] [1465798161.831384738]: Adding tcp socket [25] to pollset
[DEBUG] [1465798161.831429846]: TCPROS received a connection from [127.0.0.1:53257]
[DEBUG] [1465798161.831497080]: Connection: Creating ServiceClientLink for service [/move_base/set_logger_level] connected to [callerid=[/rosservice] address=[TCPROS connection on port 49974 to [127.0.0.1:53257 on socket 25]]]
[DEBUG] [1465798161.831534273]: Service client [/rosservice] wants service [/move_base/set_logger_level] with md5sum [*]
[DEBUG] [1465798161.835697081]: Accepted connection on socket [6], new socket [26]
[DEBUG] [1465798161.835935673]: Adding tcp socket [26] to pollset
[DEBUG] [1465798161.835984273]: TCPROS received a connection from [127.0.0.1:53259]
[DEBUG] [1465798161.836044021]: Connection: Creating ServiceClientLink for service [/move_base/set_logger_level] connected to [callerid=[/rqt_gui_py_node_21141] address=[TCPROS connection on port 49974 to [127.0.0.1:53259 on socket 26]]]
[DEBUG] [1465798161.836071826]: Service client [/rqt_gui_py_node_21141] wants service [/move_base/set_logger_level] with md5sum [51da076440d78ca1684d36c868df61ea]
[DEBUG] [1465798161.836111971]: Socket [25] received 0/4 bytes, closing
[DEBUG] [1465798161.836170231]: TCP socket [25] closed
It appears to me that everything is working correctly. The local and global plans can be visualised in rviz and they appear correct. The local planner even says: “A valid velocity command of (0.10, 0.00, -0.22) was found for this cycle.” I'm completely stumped about why the robot isn't moving. Any ideas would be greatly appreciated! (Also sorry for the code dump, hope it doesn't make the post too long).
Originally posted by bemery94 on ROS Answers with karma: 11 on 2016-06-13
Post score: 0