自主导航系列18-ros插件

2020-9-3

1，一个bug

在编译

sub1= n.subscribe("/cmd_vel", 1,SimpleLayer::openpoints);

时，出现报错

no matching function for call to ‘ros::NodeHandle::subscribe(

使用

sub1= n.subscribe("/cmd_vel", 1, & SimpleLayer::openpoints,this);

编译通过。主要的问题在于在类里面要声明this，才能找的到

第二点在于一定要将句柄初始化在public里面，这样这个订阅其他的成员才能看得到。

2，用点位触发障碍物的设置

src

#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("yes I heard"); std::cout<<pose.header.frame_id<<std::endl; } 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(0, 1, mx, my)){ master_grid.setCost(mx, my, LETHAL_OBSTACLE); } std::cout<<mx<<std::endl; std::cout<<my<<std::endl;ROS_INFO("soso"); } } // end namespace

h

#ifndef SIMPLE_LAYER_H_ #define SIMPLE_LAYER_H_ #include <ros/ros.h> #include <costmap_2d/layer.h> #include <costmap_2d/layered_costmap.h> #include <costmap_2d/GenericPluginConfig.h> #include <dynamic_reconfigure/server.h> #include <vector> #include <string> #include <iostream> #include "std_msgs/String.h" #include "geometry_msgs/PoseStamped.h" namespace simple_layer_namespace { class SimpleLayer : public costmap_2d::Layer { public: SimpleLayer(); virtual void onInitialize(); virtual void updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y, double* max_x, double* max_y); virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); void openpoints(const geometry_msgs::PoseStamped &pose); ros::Subscriber sub1; private: void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level); double mark_x_, mark_y_; dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_; }; } #endif #ifndef SIMPLE_LAYER_H_ #define SIMPLE_LAYER_H_ #include <ros/ros.h> #include <costmap_2d/layer.h> #include <costmap_2d/layered_costmap.h> #include <costmap_2d/GenericPluginConfig.h> #include <dynamic_reconfigure/server.h> #include <vector> #include <string> #include <iostream> #include "std_msgs/String.h" #include "geometry_msgs/PoseStamped.h" namespace simple_layer_namespace { class SimpleLayer : public costmap_2d::Layer { public: SimpleLayer(); virtual void onInitialize(); virtual void updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y, double* max_x, double* max_y); virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); void openpoints(const geometry_msgs::PoseStamped &pose); ros::Subscriber sub1; private: void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level); double mark_x_, mark_y_; dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_; }; } #endif