Chinaunix首页 | 论坛 | 博客
  • 博客访问: 3967376
  • 博文数量: 1095
  • 博客积分: 12961
  • 博客等级: 上将
  • 技术积分: 13124
  • 用 户 组: 普通用户
  • 注册时间: 2009-01-09 11:25
个人简介

偷得浮生半桶水(半日闲), 好记性不如抄下来(烂笔头). 信息爆炸的时代, 学习是一项持续的工作.

文章分类

全部博文(1095)

文章存档

2019年(71)

2018年(81)

2017年(80)

2016年(70)

2015年(52)

2014年(41)

2013年(51)

2012年(85)

2011年(45)

2010年(231)

2009年(288)

分类: 其他平台

2019-05-13 18:28:18

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

点击(此处)折叠或打开

  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>

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

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

点击(此处)折叠或打开

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

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

  13. ################################################
  14. ## Declare ROS messages, services and actions ##
  15. ################################################

  16. ################################################
  17. ## Declare ROS dynamic reconfigure parameters ##
  18. ################################################

  19. ###################################
  20. ## catkin specific configuration ##
  21. ###################################
  22. catkin_package(
  23. )

  24. ###########
  25. ## Build ##
  26. ###########
  27. include_directories(
  28.   include
  29.   ${catkin_INCLUDE_DIRS}
  30. )

  31. ## Declare a C++ library
  32. add_library(simple_layer
  33.   src/simple_layer.cpp
  34. )
  35. add_library(grid_layer
  36.   src/grid_layer.cpp
  37. )
  38. add_dependencies(simple_layer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
  39. add_dependencies(grid_layer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
  40. target_link_libraries(simple_layer ${catkin_LIBRARIES})
  41. target_link_libraries(grid_layer ${catkin_LIBRARIES})
  42. install(TARGETS simple_layer grid_layer
  43.         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  44.         )

  45. add_executable(sonic_layer_node src/sonic_layers.cpp)
  46. add_dependencies(sonic_layer_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
  47. 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"

  5. #include "big_msgs/ChassisSensorState.h"

  6. #include <time.h>
  7. #include <sstream>
  8. #include <tf/transform_broadcaster.h>

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

  14. boost::shared_ptr<G_CTX_T> gCtx;

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

  35.     gCtx->sonar0_pub.publish(msg);
  36.     ROS_INFO("Sonic:%lf", msg.range);
  37. }

  38. int main(int argc, char **argv)
  39. {
  40.   ros::init(argc, argv, "UltraSonicLayers");
  41.   // Exit if no ROS
  42.   if (!ros::isInitialized()) {
  43.     return 0;
  44.   }

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

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

  49.   ros::Rate loop_rate(100);
  50.   while (ros::ok())
  51.   {
  52.     ros::spinOnce();
  53.     loop_rate.sleep();
  54.   }
  55.   return 0;
  56. }
自定义图层代码

点击(此处)折叠或打开

  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

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

  5. inflation_radius: 0.5 # Shallow water 浅水区
  6. cost_scaling_factor: 3.0

  7. map_type: costmap
  8. observation_sources: scan
  9. scan: {sensor_frame: base_scan, data_type: LaserScan, topic: /scan, marking: true, clearing: true}

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

点击(此处)折叠或打开

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

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

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

点击(此处)折叠或打开

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

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

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

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



阅读(1436) | 评论(0) | 转发(0) |
给主人留下些什么吧!~~
评论热议
请登录后评论。

登录 注册