自主导航系列19-costmap与rostopic
2020-9-4
1，插件安装
新建一个ros功能包，catkin_make的时候输出成库文件在move_base里面修改参数配置文件，使得move_base引用这个插件自行新建一个发布坐标点的节点
配置文件添加以下代码
costmap_common_params
obstacles:
observation_sources: scan
scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}
global_costmap_params
map_type: costmap
plugins:
- {name: static_map, type: "costmap_2d::StaticLayer"}
- {name: obstacles, type: "costmap_2d::VoxelLayer"}
- {name: simplelayer, type: "simple_layer_namespace::SimpleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
local_costmap_params
map_type: costmap
2，代码使用
roslaunch mrobot_gazebo jian_robot_gazebo.launch
roslaunch mrobot_navigation jian_move_base.launch
rosrun mrobot_navigation set_obstacle_points
3，修改插件，满足接收障碍物点的要求
#include<simple_layers/simple_layer.h>
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::SimpleLayer, costmap_2d::Layer)
using costmap_2d::LETHAL_OBSTACLE;
namespace simple_layer_namespace
{
SimpleLayer::SimpleLayer() {
}
void SimpleLayer::onInitialize()
{
ros::NodeHandle nh("~/" + name_);
sub1= nh.subscribe("/set_obstacle_points", 1, & SimpleLayer::openpoints,this);
ros::spinOnce();
current_ = true;
dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
&SimpleLayer::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
}
void SimpleLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
enabled_ = config.enabled;
}
void SimpleLayer::updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (!enabled_)
return;
// mark_x_ = origin_x + cos(origin_yaw);
// mark_y_ = origin_y + sin(origin_yaw);
*min_x = std::min(*min_x, mark_x_);
*min_y = std::min(*min_y, mark_y_);
*max_x = std::max(*max_x, mark_x_);
*max_y = std::max(*max_y, mark_y_);
}
void SimpleLayer::openpoints(const geometry_msgs::PoseStamped &pose){
ROS_INFO("i got the pose message");
if(pose.header.frame_id=="obstacle"){
pose_update = pose;
std::cout<<pose_update.header.frame_id<<std::endl;
mark_x_ = pose_update.pose.position.x;
mark_y_ = pose_update.pose.position.y;
}
}
void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
if (!enabled_)
return;
unsigned int mx;
unsigned int my;
if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
master_grid.setCost(mx, my, LETHAL_OBSTACLE);
}
std::cout<<mark_x_<<" "<<mark_y_<<std::endl;
}
} // end namespace
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-4htzMIEB-1599208131542)(/home/jianzhuozhu/.config/Typora/typora-user-images/image-20200904101827133.png)]
