但是对于我的机器人使用了UWB进行定位, 激光雷达所做的地图被手动改动了很多, 而且加入了 sonar layer之后, 会有很大的噪点障碍. 用rotate_recovery比较难于恢复和定位. 需要改写.
点击(此处)折叠或打开
- #include <ros/ros.h>
- #include <nav_core/recovery_behavior.h>
- #include <costmap_2d/costmap_2d_ros.h>
- #include <boost/thread.hpp>
- #include <dynamic_reconfigure/Reconfigure.h>
- namespace custom_recovery_behavior
- {
- class CustomRecoveryAction : public nav_core::RecoveryBehavior
- {
- public:
- CustomRecoveryAction();
- ~CustomRecoveryAction();
- /// Initialize the parameters of the behavior
- void initialize (std::string n, tf::TransformListener* tf,
- costmap_2d::Costmap2DROS* global_costmap,
- costmap_2d::Costmap2DROS* local_costmap);
- /// Run the behavior
- void runBehavior();
- private:
- bool initialized_;
- ros::Publisher custom_recovery_reset; //costmap/plan/pose 重置
- };
- }
点击(此处)折叠或打开
- #include <custom_recovery_behavior.h>
- #include <pluginlib/class_list_macros.h>
- #include <costmap_2d/obstacle_layer.h>
- #include <std_msgs/Empty.h>
- PLUGINLIB_EXPORT_CLASS(custom_recovery_behavior::CustomRecoveryAction, nav_core::RecoveryBehavior)
- namespace custom_recovery_behavior
- {
- CustomRecoveryAction::CustomRecoveryAction(): initialized_(false) {}
- CustomRecoveryAction::~CustomRecoveryAction() {}
- void CustomRecoveryAction::initialize (std::string n, tf::TransformListener* tf,
- costmap_2d::Costmap2DROS* global_costmap,
- costmap_2d::Costmap2DROS* local_costmap)
- {
- //声明topic, 通知外部app, 清除costmap并重新定位.
- ros::NodeHandle nh;
- custom_recovery_reset = nh.advertise<std_msgs::Empty>("custom_recovery_reset", 1);
- initialized_ = true;
- ROS_WARN("Custom recovery behavior initialize.");
- }
- //自定义 RecoveryBehavior 的动作.
- void CustomRecoveryAction::runBehavior()
- {
- if(!initialized_)
- {
- ROS_ERROR("Custom recovery behavior has not been initialized, doing nothing.");
- return;
- }
- ROS_WARN("Custom recovery behavior started.");
- std_msgs::Empty msg;
- custom_recovery_reset.publish(msg);
- }
- }
然后使用uwb的位置更新机器人位置.
g_rt_ctx->pub_initalpose = nh.advertise("initialpose", 1);
点击(此处)折叠或打开
- void clear_costmaps(void)
- {
- //clear costmaps.
- std_srvs::Empty srv;
- g_rt_ctx->srv_clear_costmap.call(srv);
- }
- void set_robot_initialpose(double x, double y, double th)
- {
- geometry_msgs::PoseWithCovarianceStamped init_pose;
- init_pose.header.frame_id = "map"; //基于Map的点.
- init_pose.header.stamp = ros::Time::now();
- init_pose.pose.pose.position.x = x; //起始位置点
- init_pose.pose.pose.position.y = y;
-
- tf::Quaternion q;
- //th 调校到 -180 , 180 区间
- th = (th <= 180)? th : (th - 360);
- double arc = (th) * M_PI/180;
- q.setRPY(0, 0, arc);
- init_pose.pose.pose.orientation.x=q[0];
- init_pose.pose.pose.orientation.y=q[1];
- init_pose.pose.pose.orientation.z=q[2];
- init_pose.pose.pose.orientation.w=q[3];
- g_rt_ctx->pub_initalpose.publish(init_pose);
- clear_costmaps();
- }
配置的部分比较重要
package.
点击(此处)折叠或打开
- <?="1.0"?>
- <package format="2">
- <name>big_navigation</name>
- <version>1.2.0</version>
- <description>
- The big_navigation provides roslaunch scripts for starting the navigation.
- </description>
- <author email="iibull@yeah.net">iibull</author>
- <license>Apache 2.0</license>
- <maintainer email="iibull@yeah.net">iibull</maintainer>
- <buildtool_depend>catkin</buildtool_depend>
- <exec_depend>amcl</exec_depend>
- <exec_depend>map_server</exec_depend>
- <exec_depend>move_base</exec_depend>
- <exec_depend>big_bringup</exec_depend>
- <build_depend>message_generation</build_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>std_msgs</build_depend>
- <build_export_depend>roscpp</build_export_depend>
- <build_export_depend>std_msgs</build_export_depend>
- <build_export_depend>message_generation</build_export_depend>
- <exec_depend>roscpp</exec_depend>
- <exec_depend>std_msgs</exec_depend>
- <depend>big_msgs</depend>
- <depend>sensor_msgs</depend>
- <depend>tf</depend>
- <build_depend>cmake_modules</build_depend>
- <build_depend>pluginlib</build_depend>
- <exec_depend>pluginlib</exec_depend>
- <build_depend>nav_core</build_depend>
- <exec_depend>nav_core</exec_depend>
- <build_depend>costmap_2d</build_depend>
- <exec_depend>costmap_2d</exec_depend>
- <build_depend>geometry_msgs</build_depend>
- <exec_depend>geometry_msgs</exec_depend>
- <export>
- <nav_core plugin="${prefix}/recovery_plugin. />
- </export>
- </package>
点击(此处)折叠或打开
- ################################################################################
- # Set minimum required version of cmake, project name and compile options
- ################################################################################
- cmake_minimum_required(VERSION 2.8.3)
- project(big_navigation)
- ################################################################################
- # Find catkin packages and libraries for catkin and system dependencies
- ################################################################################
- find_package(catkin REQUIRED COMPONENTS
- message_generation
- roscpp
- std_msgs
- big_msgs
- tf
- sensor_msgs
- cmake_modules
- costmap_2d
- geometry_msgs
- nav_core
- pluginlib
- )
- ################################################################################
- # Setup for python modules and scripts
- ################################################################################
- ################################################################################
- # Declare ROS messages, services and actions
- ################################################################################
- ################################################################################
- # Declare ROS dynamic reconfigure parameters
- ################################################################################
- ################################################################################
- # Declare catkin specific configuration to be passed to dependent projects
- ################################################################################
- catkin_package(
- INCLUDE_DIRS include
- LIBRARIES custom_recovery_behavior
- CATKIN_DEPENDS message_generation roscpp std_msgs big_msgs sensor_msgs tf geometry_msgs nav_core pluginlib
- # DEPENDS system_lib
- )
- ################################################################################
- # Build
- ################################################################################
- find_package(Eigen3 REQUIRED)
- find_package(PCL REQUIRED)
- remove_definitions(-DDISABLE_LIBUSB-1.0)
- find_package(Boost REQUIRED COMPONENTS thread)
- include_directories(
- include
- ${catkin_INCLUDE_DIRS}
- ${catkin_INCLUDE_DIRS}
- ${EIGEN3_INCLUDE_DIRS}
- ${PCL_INCLUDE_DIRS}
- )
- add_definitions(${EIGEN3_DEFINITIONS})
- add_library(custom_recovery_behavior src/custom_recovery_behavior.cpp)
- add_dependencies(custom_recovery_behavior ${catkin_EXPORTED_TARGETS})
- target_link_libraries(custom_recovery_behavior
- ${Boost_LIBRARIES}
- ${catkin_LIBRARIES}
- )
- ################################################################################
- # Install
- ################################################################################
- install(DIRECTORY launch param rviz
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
- )
- install(TARGETS custom_recovery_behavior
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- )
- install(FILES recovery_plugin.
- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
- )
- ## Install project namespaced headers
- install(DIRECTORY include/
- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
- FILES_MATCHING PATTERN "*.h"
- #PATTERN ".svn" EXCLUDE
- )
- ################################################################################
- # Test
- ################################################################################
点击(此处)折叠或打开
- <library path="lib/libcustom_recovery_behavior">
- <class name="custom_recovery_behavior/CustomRecoveryAction" type="custom_recovery_behavior::CustomRecoveryAction" base_class_type="nav_core::RecoveryBehavior">
- <description>
- 自定义的recovery behavior. 用于清除costmap,限定运行速度.
- </description>
- </class>
- </library>
点击(此处)折叠或打开
- <launch>
- <arg name="model" default="$(env BIG_MODEL)" doc="Waffle"/>
- <arg name="cmd_vel_topic" default="/cmd_vel" />
- <arg name="odom_topic" default="odom" />
- <arg name="move_forward_only" default="false"/>
- <arg name="PlannerROS" default="DWAPlannerROS" />
- <rosparam param="recovery_behaviors"> [ { name: "custom_recovery_behavior", type: "custom_recovery_behavior/CustomRecoveryAction" } ] </rosparam>
- <!-- move_base -->
- <group if="$(eval arg('PlannerROS') == 'DWAPlannerROS')">
- <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
- <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
- <rosparam file="$(find big_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
- <rosparam file="$(find big_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
- <rosparam file="$(find big_navigation)/param/local_costmap_params.yaml" command="load" />
- <rosparam file="$(find big_navigation)/param/global_costmap_params.yaml" command="load" />
- <rosparam file="$(find big_navigation)/param/move_base_params.yaml" command="load" />
- <rosparam file="$(find big_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
- <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
- <remap from="odom" to="$(arg odom_topic)"/>
- <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
- </node>
- </group>
- <group if="$(eval arg('PlannerROS') == 'TrajectoryPlannerROS')">
- <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
- <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
- <rosparam file="$(find big_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
- <rosparam file="$(find big_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
- <rosparam file="$(find big_navigation)/param/local_costmap_params.yaml" command="load" />
- <rosparam file="$(find big_navigation)/param/global_costmap_params.yaml" command="load" />
- <rosparam file="$(find big_navigation)/param/move_base_params.yaml" command="load" />
- <rosparam file="$(find big_navigation)/param/base_local_planner_params.yaml" command="load" />
- <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
- <remap from="odom" to="$(arg odom_topic)"/>
- <param name="TrajectoryPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
- </node>
- </group>
- </launch>
点击(此处)折叠或打开
- * /move_base/planner_patience: 5.0
* /move_base/shutdown_costmaps: False
* /recovery_behaviors: [{'type': 'custom...
* /robot_description: