ROS costmap_2d 插件问题

12560阅读　0评论2019-05-13　iibull
分类：其他平台

超声波/自定义图层的插件.

点击(此处)折叠或打开

  1. <?xml version="1.0"?>
  2. <package format="2">
  3.   <name>big_layers</name>
  4.   <version>0.0.0</version>
  5.   <description>The big_layers package</description>
  6.   <maintainer email="ray@todo.todo">ray</maintainer>
  7.   <license>TODO</license>

  9.   <buildtool_depend>catkin</buildtool_depend>
  10.   <build_depend>costmap_2d</build_depend>
  11.   <build_depend>dynamic_reconfigure</build_depend>
  12.   <build_depend>roscpp</build_depend>
  13.   <build_depend>sensor_msgs</build_depend>
  14.   <build_depend>tf</build_depend>
  15.   <build_depend>rospy</build_depend>
  16.   <build_depend>std_msgs</build_depend>
  17.   <build_export_depend>costmap_2d</build_export_depend>
  18.   <build_export_depend>dynamic_reconfigure</build_export_depend>
  19.   <build_export_depend>roscpp</build_export_depend>
  20.   <build_export_depend>sensor_msgs</build_export_depend>
  21.   <build_export_depend>tf</build_export_depend>
  22.   <build_export_depend>rospy</build_export_depend>
  23.   <build_export_depend>std_msgs</build_export_depend>
  24.   <exec_depend>costmap_2d</exec_depend>
  25.   <exec_depend>dynamic_reconfigure</exec_depend>
  26.   <exec_depend>roscpp</exec_depend>
  27.   <exec_depend>sensor_msgs</exec_depend>
  28.   <exec_depend>tf</exec_depend>
  29.   <exec_depend>rospy</exec_depend>
  30.   <exec_depend>std_msgs</exec_depend>
  31.   <depend>big_msgs</depend>

  33.   <export>
  34.     <!-- Other tools can request additional information be placed here -->
  35.     <costmap_2d plugin="${prefix}/costmap_plugins.xml" />
  36.   </export>
  37. </package>

点击(此处)折叠或打开

  1. cmake_minimum_required(VERSION 2.8.3)
  2. project(big_layers)

  4. find_package(catkin REQUIRED COMPONENTS
  5.   costmap_2d
  6.   dynamic_reconfigure
  7.   roscpp
  8.   sensor_msgs
  9.   tf
  10.   big_msgs
  11.   rospy
  12.   std_msgs
  13. )

  15. ################################################
  16. ## Declare ROS messages, services and actions ##
  17. ################################################

  19. ################################################
  20. ## Declare ROS dynamic reconfigure parameters ##
  21. ################################################

  23. ###################################
  24. ## catkin specific configuration ##
  25. ###################################
  26. catkin_package(
  27. )

  29. ###########
  30. ## Build ##
  31. ###########
  32. include_directories(
  33.   include
  34.   ${catkin_INCLUDE_DIRS}
  35. )

  37. ## Declare a C++ library
  38. add_library(simple_layer
  39.   src/simple_layer.cpp
  40. )
  41. add_library(grid_layer
  42.   src/grid_layer.cpp
  43. )
  44. add_dependencies(simple_layer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
  45. add_dependencies(grid_layer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
  46. target_link_libraries(simple_layer ${catkin_LIBRARIES})
  47. target_link_libraries(grid_layer ${catkin_LIBRARIES})
  48. install(TARGETS simple_layer grid_layer
  49.         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  50.         )

  52. add_executable(sonic_layer_node src/sonic_layers.cpp)
  53. add_dependencies(sonic_layer_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
  54. target_link_libraries(sonic_layer_node
       ${catkin_LIBRARIES}
     )
    #############
    ## Install ##
    #############
    install(TARGETS sonic_layer_node
       ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
       LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
       RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
     )
    install(FILES
       costmap_plugins.xml
       DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
    )

超声波源码.

点击(此处)折叠或打开

  1. /* sonic_layers.cpp, 从前后两个超声波获得信息,并通过topic发布到 local_costmap中去 */
  2. #include "ros/ros.h"
  3. #include "sensor_msgs/Range.h"
  4. #include "std_msgs/Header.h"

  6. #include "big_msgs/ChassisSensorState.h"

  8. #include <time.h>
  9. #include <sstream>
  10. #include <tf/transform_broadcaster.h>

  12. typedef struct {
  13.   ros::Subscriber sub_sensors; //碰撞/防跌落信息.
  14.   ros::Publisher sonar0_pub;
  15.   tf::TransformBroadcaster broadcaster;
  16. } G_CTX_T;

  18. boost::shared_ptr<G_CTX_T> gCtx;

  20. void cbSensors(const big_msgs::ChassisSensorState sensor)
  21. {
  22.      //std::cout << "SensorMessage: " << msg << std::endl ;
  23.     sensor_msgs::Range msg;
  24.     std_msgs::Header header;
  25.     header.stamp = ros::Time::now();
  26.     header.frame_id = "sonar0";
  27.     msg.header = header;
  28.     msg.field_of_view = 1;
  29.     msg.min_range = 0;
  30.     msg.max_range = 5;
  31.     msg.range = sensor.sonar / 1000; //毫米 -> 米
  32.     gCtx->broadcaster.sendTransform(
  33.       tf::StampedTransform(
  34.         tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
  35.         ros::Time::now(),
  36.         "base_link",
  37.         "sonar0"
  38.       )
  39.     );

  41.     gCtx->sonar0_pub.publish(msg);
  42.     ROS_INFO("Sonic:%lf", msg.range);
  43. }

  45. int main(int argc, char **argv)
  46. {
  47.   ros::init(argc, argv, "UltraSonicLayers");
  48.   // Exit if no ROS
  49.   if (!ros::isInitialized()) {
  50.     return 0;
  51.   }

  53.   gCtx.reset(new G_CTX_T());

  55.   ros::NodeHandle n;
  56.   gCtx->sonar0_pub = n.advertise<sensor_msgs::Range>("/sonar0", 10);
  57.   gCtx->sub_sensors = n.subscribe("/sensor_state", 1, cbSensors); // //碰撞/防跌落信息.

  59.   ros::Rate loop_rate(100);
  60.   while (ros::ok())
  61.   {
  62.     ros::spinOnce();
  63.     loop_rate.sleep();
  64.   }
  65.   return 0;
  66. }
自定义图层代码

点击(此处)折叠或打开

  1. #include <big_layers/grid_layer.h>
  2. #include <pluginlib/class_list_macros.h>
  3.  
  4. PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::GridLayer, costmap_2d::Layer)
  5.  
  6. using costmap_2d::LETHAL_OBSTACLE;
  7. using costmap_2d::NO_INFORMATION;
  8. using costmap_2d::FREE_SPACE;
  9.  
  10. namespace simple_layer_namespace
  11. {
  12.  
  13. unsigned flag = 0;
  14.  
  15. GridLayer::GridLayer() {}
  16.  
  17. void GridLayer::onInitialize()
  18. {
  19.   ros::NodeHandle nh("~/" + name_);
  20.   current_ = true;
  21.   default_value_ = NO_INFORMATION;
  22.   matchSize();
  23.  
  24.   dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
  25.   dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
  26.       &GridLayer::reconfigureCB, this, _1, _2);
  27.   dsrv_->setCallback(cb);
  28. }
  29.  
  30.  
  31. void GridLayer::matchSize()
  32. {
  33.   Costmap2D* master = layered_costmap_->getCostmap();
  34.   resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
  35.             master->getOriginX(), master->getOriginY());
  36. }
  37.  
  38.  
  39. void GridLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
  40. {
  41.   enabled_ = config.enabled;
  42. }
  43.  
  44. void GridLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
  45.                                            double* min_y, double* max_x, double* max_y)
  46. {
  47.   if (!enabled_)
  48.     return;
  49.   
  50.   if (flag == 0)
  51.   {
  52.      flag = 1;
  53.   }else
  54.     return;
  55.  
  56.   double mark_x = robot_x + cos(robot_yaw);
  57.   double mark_y = robot_y + sin(robot_yaw);
  58.   unsigned int mx;
  59.   unsigned int my;
  60.   if(worldToMap(mark_x, mark_y, mx, my)){
  61.      setCost(mx, my,LETHAL_OBSTACLE);
  62.   }
  63.   
  64.   *min_x = std::min(*min_x, mark_x);
  65.   *min_y = std::min(*min_y, mark_y);
  66.   *max_x = std::max(*max_x, mark_x);
  67.   *max_y = std::max(*max_y, mark_y);
  68. }
  69.  
  70. void GridLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
  71.                                           int max_j)
  72. {
  73.   if (!enabled_)
  74.     return;
  75.  
  76.   for (int j = min_j; j < max_j; j++)
  77.   {
  78.     for (int i = min_i; i < max_i; i++)
  79.     {
  80.       int index = getIndex(i, j);
  81.       if (costmap_[index] == NO_INFORMATION)
  82.         continue;
  83.       master_grid.setCost(i, j, costmap_[index]);
  84.     }
  85.   }
  86. }
  87.  
  88. } // end namespace
最最重要的是图层的配置加载问题.

点击(此处)折叠或打开

  1. obstacle_range: 3.0 #检测在区域内的障碍, 超过此距离, 默认用SLAM的地图的内容.
  2. raytrace_range: 3.5

  4. #footprint: [[-0.3, -0.3], [-0.3, 0.3], [0.3, 0.3], [0.3, -0.3]]
  5. robot_radius: 0.3 #危险区域的距离.

  7. inflation_radius: 0.5 # Shallow water 浅水区
  8. cost_scaling_factor: 3.0

  10. map_type: costmap
  11. observation_sources: scan
  12. scan: {sensor_frame: base_scan, data_type: LaserScan, topic: /scan, marking: true, clearing: true}

  14. sonar_sensor_layer: 
  15.   clear_threshold: 0.46 #概率低于clear_threshold的单元格被标记为空闲空间
  16.   mark_threshold: 0.98 #概率高于mark_threshold的细胞被标记为致命障碍
  17.   topics: ["/sonar0", "/sonar1"] # 订阅超神波主题，添加到sonar_layer到 costmap中
  18.   clear_on_max_reading: true #是否清楚超过最大范围的数值.
  19.   no_readings_timeout: 0.0

点击(此处)折叠或打开

  1. local_costmap:
  2.   global_frame: /odom
  3.   robot_base_frame: /base_footprint

  5.   update_frequency: 8.0
  6.   publish_frequency: 8.0
  7.   transform_tolerance: 0.5

  9.   static_map: false
  10.   rolling_window: true
  11.   width: 3
  12.   height: 3
  13.   resolution: 0.05
  14.   
  15.   plugins:  #缩进很重要
  16.     - {name: sonar_sensor_layer, type: "range_sensor_layer::RangeSensorLayer"}

点击(此处)折叠或打开

  1. global_costmap:
  2.   global_frame: /map
  3.   robot_base_frame: /base_footprint

  5.   update_frequency: 3.0
  6.   publish_frequency: 3.0
  7.   transform_tolerance: 0.5

  9.   static_map: true
  10.  
  11.   plugins: #缩进很重要
  12.     - {name: gridlayer, type: "simple_layer_namespace::GridLayer"}

big_layers.zip  包括了自定图层以及navigation的param设置.



上一篇：ROS程序开机自启动
下一篇：自定义ROS layer的调用日志分析