自主导航系列18-ros插件机制

自主导航系列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