How to subscribe a new costmap2dROS object to the global_costmap/costmap
Hi all,
I am planning to write my own node in ROS and I need to access the global_costmap
which is being published. For that, I have created a new Costmap2DROS object:
int main(int argc, char** argv){
ros::init(argc,argv,"publish_multiple_goals");
ros::NodeHandle n;
tf::TransformListener tf_(ros::Duration(10));
costmap_ros_ = new costmap_2d::Costmap2DROS("my_costmap", tf_);
costmap_ros_->start();
costmap_ = costmap_ros_->getCostmap();
return 0;
}
Now, I have a my_costmap_params.yaml
file where I have specified the map_topic
: /move_base_node/global_costmap/costmap
(this is being published by move_base) so that the new costmap2DROS object subscribes to it:
my_costmap:
#Set the global and robot frames for the costmap
global_frame: /map
robot_base_frame: /base_link
#Set the update and publish frequency of the costmap
update_frequency: 2.0
publish_frequency: 2.0
#We'll use a map served by the map_server to initialize this costmap
static_map: true
rolling_window: false
use_dijkstra: false
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
static_layer:
map_topic: /move_base_node/global_costmap/costmap
#Configuration for the sensors that the costmap will use to update a map
obstacle_layer:
observation_sources: base_scan
base_scan: {data_type: LaserScan, sensor_frame: /base_laser_link_1, topic: /base_scan, expected_update_rate: 0.4,
observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.0}
I am loading the above parameters to parameter_server
here:
publish_multiple.xml
<launch>
<node pkg="publish_multiple_goals" type="publish_multiple_goals" respawn="false" name="publish_multiple_goals" output="screen">
<rosparam file="$(find drsim_control)/move_base_config/my_costmap_params.yaml" command="load" />
</node>
</launch>
Now the problem is it still subscribes to the "map" topic published by the map_server
and does not subscribe to the map_topic
I mentioned above that is /move_base_node/global_costmap/costmap
using the costmap2DROS object I created above. I want to make sure that the costmap2DROS object created above subscribes to the map_topic
to get the global_costmap
. Does anyone have any idea what is going wrong here or what can I do to achieve the above mentioned task? Let me know if you need more information from me.
Update:
I think I am able to subscribe to the global_costmap
but still when I use functions like: costmap_ros_->getRobotFootprint()
, it gives me wrong readings (when I use this function in the global planner plugin, it gives me the correct values). Also, the Cost values of the costmap is either 0 or 254. Shouldn't the inflation be there according to http://wiki.ros.org/costmap_2d#Inflation ? Any help will be appreciated.
Thanks in advance.
Naman Kumar