ROS下用代码指定坐标完成导航任务

喜欢ヅ旅行 2023-10-10 19:28 5阅读 0赞

我现在有这样一个需求：使用代码导航到指定位置(该位置由人工指定坐标)，之前的导航都是直接使用rviz上面的可视化选点，这篇文章主要介绍下代码的实现逻辑。  
  我的环境：Ubuntu18.04+京天Turtlebot3仿真环境

### 程序源码 ###

#include <ros/ros.h>
    #include <move_base_msgs/MoveBaseAction.h>
    #include <actionlib/client/simple_action_client.h>
    typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
    
     
    int main(int argc, char** argv){
        
    
        ros::init(argc, argv, "simple_navigation_goals");
    
        //tell the action client that we want to spin a thread by default
        MoveBaseClient ac("move_base", true);
    
        //wait for the action server to come up
        while(!ac.waitForServer(ros::Duration(5.0))){
        
            ROS_INFO("Waiting for the move_base action server to come up");
        }
    
        move_base_msgs::MoveBaseGoal goal;
    
        //we'll send a goal to the robot to move 1 meter forward
        goal.target_pose.header.frame_id = "base_link";
        goal.target_pose.header.stamp = ros::Time::now();
        goal.target_pose.pose.position.x = 1.0;
        goal.target_pose.pose.orientation.w = 1.0;
        ROS_INFO("Sending goal");
        ac.sendGoal(goal);
        ac.waitForResult();
    
    
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            ROS_INFO("Hooray, the base moved 1 meter forward");
        else
            ROS_INFO("The base failed to move forward 1 meter for some reason");
        return 0;
    
    }

### CMakeListx.txt ###

cmake_minimum_required(VERSION 3.0.2)
    project(simple_navigation_goals)
    
    ## Compile as C++11, supported in ROS Kinetic and newer
    # add_compile_options(-std=c++11)
    
    ## Find catkin macros and libraries
    ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
    ## is used, also find other catkin packages
    find_package(catkin REQUIRED COMPONENTS
      actionlib
      move_base_msgs
      roscpp
    )
    
    ## System dependencies are found with CMake's conventions
    # find_package(Boost REQUIRED COMPONENTS system)
    
    
    ## Uncomment this if the package has a setup.py. This macro ensures
    ## modules and global scripts declared therein get installed
    ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
    # catkin_python_setup()
    
    ################################################
    ## Declare ROS messages, services and actions ##
    ################################################
    
    ## To declare and build messages, services or actions from within this
    ## package, follow these steps:
    ## * Let MSG_DEP_SET be the set of packages whose message types you use in
    ##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
    ## * In the file package.xml:
    ##   * add a build_depend tag for "message_generation"
    ##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
    ##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
    ##     but can be declared for certainty nonetheless:
    ##     * add a exec_depend tag for "message_runtime"
    ## * In this file (CMakeLists.txt):
    ##   * add "message_generation" and every package in MSG_DEP_SET to
    ##     find_package(catkin REQUIRED COMPONENTS ...)
    ##   * add "message_runtime" and every package in MSG_DEP_SET to
    ##     catkin_package(CATKIN_DEPENDS ...)
    ##   * uncomment the add_*_files sections below as needed
    ##     and list every .msg/.srv/.action file to be processed
    ##   * uncomment the generate_messages entry below
    ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
    
    ## Generate messages in the 'msg' folder
    # add_message_files(
    #   FILES
    #   Message1.msg
    #   Message2.msg
    # )
    
    ## Generate services in the 'srv' folder
    # add_service_files(
    #   FILES
    #   Service1.srv
    #   Service2.srv
    # )
    
    ## Generate actions in the 'action' folder
    # add_action_files(
    #   FILES
    #   Action1.action
    #   Action2.action
    # )
    
    ## Generate added messages and services with any dependencies listed here
    # generate_messages(
    #   DEPENDENCIES
    #   move_base_msgs
    # )
    
    ################################################
    ## Declare ROS dynamic reconfigure parameters ##
    ################################################
    
    ## To declare and build dynamic reconfigure parameters within this
    ## package, follow these steps:
    ## * In the file package.xml:
    ##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
    ## * In this file (CMakeLists.txt):
    ##   * add "dynamic_reconfigure" to
    ##     find_package(catkin REQUIRED COMPONENTS ...)
    ##   * uncomment the "generate_dynamic_reconfigure_options" section below
    ##     and list every .cfg file to be processed
    
    ## Generate dynamic reconfigure parameters in the 'cfg' folder
    # generate_dynamic_reconfigure_options(
    #   cfg/DynReconf1.cfg
    #   cfg/DynReconf2.cfg
    # )
    
    ###################################
    ## catkin specific configuration ##
    ###################################
    ## The catkin_package macro generates cmake config files for your package
    ## Declare things to be passed to dependent projects
    ## INCLUDE_DIRS: uncomment this if your package contains header files
    ## LIBRARIES: libraries you create in this project that dependent projects also need
    ## CATKIN_DEPENDS: catkin_packages dependent projects also need
    ## DEPENDS: system dependencies of this project that dependent projects also need
    catkin_package(
    #  INCLUDE_DIRS include
    #  LIBRARIES simple_navigation_goals
    #  CATKIN_DEPENDS actionlib move_base_msgs roscpp
    #  DEPENDS system_lib
    )
    
    ###########
    ## Build ##
    ###########
    
    ## Specify additional locations of header files
    ## Your package locations should be listed before other locations
    include_directories(
    # include
      ${
        catkin_INCLUDE_DIRS}
    )
    
    ## Declare a C++ library
    # add_library(${
        PROJECT_NAME}
    #   src/${
        PROJECT_NAME}/simple_navigation_goals.cpp
    # )
    
    ## Add cmake target dependencies of the library
    ## as an example, code may need to be generated before libraries
    ## either from message generation or dynamic reconfigure
    # add_dependencies(${
        PROJECT_NAME} ${
        ${
        PROJECT_NAME}_EXPORTED_TARGETS} ${
        catkin_EXPORTED_TARGETS})
    
    ## Declare a C++ executable
    ## With catkin_make all packages are built within a single CMake context
    ## The recommended prefix ensures that target names across packages don't collide
    # add_executable(${
        PROJECT_NAME}_node src/simple_navigation_goals_node.cpp)
    
    ## Rename C++ executable without prefix
    ## The above recommended prefix causes long target names, the following renames the
    ## target back to the shorter version for ease of user use
    ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
    # set_target_properties(${
        PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
    
    ## Add cmake target dependencies of the executable
    ## same as for the library above
    # add_dependencies(${
        PROJECT_NAME}_node ${
        ${
        PROJECT_NAME}_EXPORTED_TARGETS} ${
        catkin_EXPORTED_TARGETS})
    
    ## Specify libraries to link a library or executable target against
    # target_link_libraries(${
        PROJECT_NAME}_node
    #   ${
        catkin_LIBRARIES}
    # )
    
    add_executable(send_goal src/send_goal.cpp)
    target_link_libraries(send_goal ${
        catkin_LIBRARIES})
    
    #############
    ## Install ##
    #############
    
    # all install targets should use catkin DESTINATION variables
    # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
    
    ## Mark executable scripts (Python etc.) for installation
    ## in contrast to setup.py, you can choose the destination
    # catkin_install_python(PROGRAMS
    #   scripts/my_python_script
    #   DESTINATION ${
        CATKIN_PACKAGE_BIN_DESTINATION}
    # )
    
    ## Mark executables for installation
    ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
    # install(TARGETS ${
        PROJECT_NAME}_node
    #   RUNTIME DESTINATION ${
        CATKIN_PACKAGE_BIN_DESTINATION}
    # )
    
    ## Mark libraries for installation
    ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
    # install(TARGETS ${
        PROJECT_NAME}
    #   ARCHIVE DESTINATION ${
        CATKIN_PACKAGE_LIB_DESTINATION}
    #   LIBRARY DESTINATION ${
        CATKIN_PACKAGE_LIB_DESTINATION}
    #   RUNTIME DESTINATION ${
        CATKIN_GLOBAL_BIN_DESTINATION}
    # )
    
    ## Mark cpp header files for installation
    # install(DIRECTORY include/${
        PROJECT_NAME}/
    #   DESTINATION ${
        CATKIN_PACKAGE_INCLUDE_DESTINATION}
    #   FILES_MATCHING PATTERN "*.h"
    #   PATTERN ".svn" EXCLUDE
    # )
    
    ## Mark other files for installation (e.g. launch and bag files, etc.)
    # install(FILES
    #   # myfile1
    #   # myfile2
    #   DESTINATION ${
        CATKIN_PACKAGE_SHARE_DESTINATION}
    # )
    
    #############
    ## Testing ##
    #############
    
    ## Add gtest based cpp test target and link libraries
    # catkin_add_gtest(${
        PROJECT_NAME}-test test/test_simple_navigation_goals.cpp)
    # if(TARGET ${
        PROJECT_NAME}-test)
    #   target_link_libraries(${
        PROJECT_NAME}-test ${
        PROJECT_NAME})
    # endif()
    
    ## Add folders to be run by python nosetests
    # catkin_add_nosetests(test)

### package.xml ###

<?xml version="1.0"?>
    <package format="2">
      <name>simple_navigation_goals</name>
      <version>0.0.0</version>
      <description>The simple_navigation_goals package</description>
    
      
      
      
      <maintainer email="user@todo.todo">user</maintainer>
    
    
      
      
      
      <license>TODO</license>
    
    
      
      
      
      
    
    
      
      
      
      
    
    
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      <buildtool_depend>catkin</buildtool_depend>
      <build_depend>actionlib</build_depend>
      <build_depend>move_base_msgs</build_depend>
      <build_depend>roscpp</build_depend>
      <build_export_depend>actionlib</build_export_depend>
      <build_export_depend>move_base_msgs</build_export_depend>
      <build_export_depend>roscpp</build_export_depend>
      <exec_depend>actionlib</exec_depend>
      <exec_depend>move_base_msgs</exec_depend>
      <exec_depend>roscpp</exec_depend>
    
      
      <export>
        
    
      </export>
    </package>

### 执行步骤 ###

roslaunch turtlebot3_gazebo turtlebot3_world.launch
    # 开启导航服务器
    roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml 
    rosrun simple_navigation_goals send_goal

参考：  
[http://www.taodudu.cc/news/show-151517.html][http_www.taodudu.cc_news_show-151517.html]  
[https://blog.csdn.net/weixin\_52602983/article/details/129837062][https_blog.csdn.net_weixin_52602983_article_details_129837062]  
[https://blog.csdn.net/woodgril/article/details/107321003][https_blog.csdn.net_woodgril_article_details_107321003]

[http_www.taodudu.cc_news_show-151517.html]: http://www.taodudu.cc/news/show-151517.html
[https_blog.csdn.net_weixin_52602983_article_details_129837062]: https://blog.csdn.net/weixin_52602983/article/details/129837062
[https_blog.csdn.net_woodgril_article_details_107321003]: https://blog.csdn.net/woodgril/article/details/107321003