first commit

CMakeLists.txt

@@ -0,0 +1,214 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(navigation)
+
+## 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
+        geometry_msgs
+        roscpp
+        rospy
+        std_msgs
+        user_msgs
+        actionlib
+        )
+
+## 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
+#   geometry_msgs#   std_msgs#   user_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 navigation
+        #  CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs user_msgs
+        #  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}/navigation.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/navigation.cpp src/main.cpp src/move_action.cpp)
+add_executable(${PROJECT_NAME}_test src/action_test.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})
+add_dependencies(${PROJECT_NAME}_test ${${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}
+        )
+target_link_libraries(${PROJECT_NAME}_test
+        ${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_navigation.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)

config/navigation_cfg.yaml

@@ -0,0 +1,14 @@
+nav_param:
+  nav_speed_max: 0.6
+  nav_speed_mid: 0.2974993
+  nav_speed_min: 0.1
+  speed_angular: 0.6      #设置机器人在原地旋转时候的角速度
+move_action_param:
+  speed_p: 0.5   #速度PID参数 P
+  speed_d: 0.1   #速度PID参数 D
+  dir_p: 0.09235011663949032     #方向PID参数 P
+  dir_d: 0.013192873805641474     #方向PID参数 D
+
+  max_line_speed: 1.0     #设置机器人最大线速度 保护措施
+  max_angular_speed: 2.0  #设置机器人最大角速度 保护措施
+  speed_angular: 0.5      #设置机器人在原地旋转时候的角速度

launch/navigation.launch

@@ -0,0 +1,11 @@
+<launch>
+    <arg name="logOutput"  default="screen" /><!-- log: 输出至文件 screen: 输出至屏幕 -->
+
+    <rosparam file="$(find navigation)/config/navigation_cfg.yaml" command="load" />
+    <node pkg="navigation" type="navigation_node" name="navigation_node" output="$(arg logOutput)">
+
+    </node>
+</launch>
+
+
+

package.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>navigation</name>
+  <version>0.0.0</version>
+  <description>The navigation package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="lenotary@todo.todo">lenotary</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/navigation</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>user_msgs</build_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>user_msgs</build_export_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>user_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/action_test.cpp

@@ -0,0 +1,257 @@
+/*
+ * @Descripttion: action_test.cpp Project
+ * @version: Project v0.1
+ * @Author: lenotary
+ * @Date: 2021/3/30 下午1:12
+ * @LastEditor: lenotary
+ * @LastEditTime: 2021/3/30 下午1:12
+ */
+
+#include <iostream>
+#include <ros/ros.h>
+#include <ros/node_handle.h>
+#include <termios.h>
+#include <csignal>
+#include <cmath>
+#include <cstdio>
+#include <cstdlib>
+#include <sys/poll.h>
+#include <thread>
+#include <utility>
+#include "actionlib/client/simple_action_client.h"
+#include "user_msgs/robot_pos_ctrlAction.h"
+#include "user_msgs/NAV_POINT.h"
+#include "sys/types.h"
+#include "unistd.h"
+#include "linux/input.h"
+#include "fcntl.h"
+#include "termio.h"
+
+#define KEYCODE_q 113
+#define KEYCODE_c 99
+#define KEYCODE_Q 81
+#define KEYCODE_SPACE 32
+#define KEYCODE_ENTER 10
+
+int scanKeyboard() {
+    int in;
+    struct termios new_settings;
+    struct termios stored_settings;
+    tcgetattr(0, &stored_settings);
+    new_settings = stored_settings;
+    new_settings.c_lflag &= (~ICANON);
+    new_settings.c_cc[VTIME] = 0;
+    tcgetattr(0, &stored_settings);
+    new_settings.c_cc[VMIN] = 1;
+    tcsetattr(0, TCSANOW, &new_settings);
+
+    in = getchar();
+
+    tcsetattr(0, TCSANOW, &stored_settings);
+    return in;
+}
+
+class CActionTest {
+public:
+    explicit CActionTest(ros::NodeHandle &nh);
+
+private:
+    void DoneCb(const actionlib::SimpleClientGoalState &state, const user_msgs::robot_pos_ctrlResultConstPtr &result);
+
+    void ActiveCb();
+
+    void FeedbackCb(const user_msgs::robot_pos_ctrlFeedbackConstPtr &msg);
+
+    std::vector<user_msgs::NAV_POINT> CreatPath(std::vector<int> id, std::vector<int> dir, std::vector<int> type);
+
+    user_msgs::robot_pos_ctrlGoal CreatGoal(std::vector<user_msgs::NAV_POINT> path);
+
+    void KeyGetLoop();
+
+private:
+    bool m_bCancelGoal_{false};
+    bool m_bIsActive_{false};
+    bool m_bPause_{false};
+    int m_nGoalNum_{0};
+    int m_nGoalNumLast_{0};
+    int m_nMaxGoalNum_{4};
+
+    user_msgs::robot_pos_ctrlGoal m_iGoal_;
+    actionlib::SimpleActionClient<user_msgs::robot_pos_ctrlAction> m_iActionClient_;
+};
+
+CActionTest::CActionTest(ros::NodeHandle &nh) : m_iActionClient_("/robot_pos_ctrl", true) {
+    std::vector<int> id0{48, 41};
+    std::vector<int> dir0{2, 2};
+    std::vector<int> type0{3, 3};
+
+    std::vector<int> id1{41, 1, 2, 3};
+    std::vector<int> dir1{2, 2, 2, 2};
+    std::vector<int> type1{3, 3, 3, 3};
+
+    std::vector<int> id2{3, 6};
+    std::vector<int> dir2{2, 3};
+    std::vector<int> type2{3, 3};
+
+    std::vector<int> id3{6, 25, 16};
+    std::vector<int> dir3{2, 2, 1};
+    std::vector<int> type3{3, 2, 3};
+
+    std::vector<int> id4{16, 21, 22, 21, 16, 25, 6, 3, 2, 1, 41, 48, 49, 48};
+    std::vector<int> dir4{2, 1, 2, 2, 3, 3, 2, 1, 2, 2, 2, 2, 2, 2};
+    std::vector<int> type4{3, 3, 5, 3, 3, 3, 3, 3, 3, 3, 3, 3, 5, 3};
+
+    auto path0 = CreatPath(id0, dir0, type0);
+    auto path1 = CreatPath(id1, dir1, type1);
+    auto path2 = CreatPath(id2, dir2, type2);
+    auto path3 = CreatPath(id3, dir3, type3);
+    auto path4 = CreatPath(id4, dir4, type4);
+//    auto path2 = CreatPath(id2, dir2, type2);
+    std::thread th1(&CActionTest::KeyGetLoop, this);
+    th1.detach();
+    ROS_INFO("等待链接 nav action 服务器  robot_pos_ctrl ...");
+    bool serverExist = m_iActionClient_.waitForServer(ros::Duration(2.0));
+    if (!serverExist) {
+        ROS_WARN("服务器连接失败");
+        return;
+    } else {
+        ROS_INFO("服务器链接成功");
+    }
+    while (ros::ok()) {
+        ros::Duration(2.0).sleep();
+        if (m_bPause_)
+            continue;
+        ROS_INFO("目标ID %d", m_nGoalNum_);
+        if (m_nGoalNum_ == 0) {
+            m_iActionClient_.sendGoal(CreatGoal(path0), [this](auto &&PH1, auto &&PH2) { DoneCb(PH1, PH2); },
+                                      [this] { ActiveCb(); }, [this](auto &&PH1) { FeedbackCb(PH1); });
+            bool finished_before_timeout = m_iActionClient_.waitForResult(ros::Duration(200));
+            if (!finished_before_timeout) {
+                ROS_WARN("任务超时 %d", m_nGoalNum_);
+                m_iActionClient_.cancelGoal();
+                m_nGoalNum_ = -1;
+            }
+        } else if (m_nGoalNum_ == 1) {
+            m_iActionClient_.sendGoal(CreatGoal(path1), [this](auto &&PH1, auto &&PH2) { DoneCb(PH1, PH2); },
+                                      [this] { ActiveCb(); }, [this](auto &&PH1) { FeedbackCb(PH1); });
+            bool finished_before_timeout = m_iActionClient_.waitForResult(ros::Duration(200));
+            if (!finished_before_timeout) {
+                ROS_WARN("任务超时 %d", m_nGoalNum_);
+                m_iActionClient_.cancelGoal();
+                m_nGoalNum_ = -1;
+            }
+        } else if (m_nGoalNum_ == 2) {
+            m_iActionClient_.sendGoal(CreatGoal(path2), [this](auto &&PH1, auto &&PH2) { DoneCb(PH1, PH2); },
+                                      [this] { ActiveCb(); }, [this](auto &&PH1) { FeedbackCb(PH1); });
+            bool finished_before_timeout = m_iActionClient_.waitForResult(ros::Duration(200));
+            if (!finished_before_timeout) {
+                ROS_WARN("任务超时 %d", m_nGoalNum_);
+                m_iActionClient_.cancelGoal();
+                m_nGoalNum_ = -1;
+            }
+        } else if (m_nGoalNum_ == 3) {
+            m_iActionClient_.sendGoal(CreatGoal(path3), [this](auto &&PH1, auto &&PH2) { DoneCb(PH1, PH2); },
+                                      [this] { ActiveCb(); }, [this](auto &&PH1) { FeedbackCb(PH1); });
+            bool finished_before_timeout = m_iActionClient_.waitForResult(ros::Duration(200));
+            if (!finished_before_timeout) {
+                ROS_WARN("任务超时 %d", m_nGoalNum_);
+                m_iActionClient_.cancelGoal();
+                m_nGoalNum_ = -1;
+            }
+        } else if (m_nGoalNum_ == 4) {
+            m_iActionClient_.sendGoal(CreatGoal(path4), [this](auto &&PH1, auto &&PH2) { DoneCb(PH1, PH2); },
+                                      [this] { ActiveCb(); }, [this](auto &&PH1) { FeedbackCb(PH1); });
+            bool finished_before_timeout = m_iActionClient_.waitForResult(ros::Duration(200));
+            if (!finished_before_timeout) {
+                ROS_WARN("任务超时 %d", m_nGoalNum_);
+                m_iActionClient_.cancelGoal();
+                m_nGoalNum_ = -1;
+            }
+        }
+    }
+    ROS_INFO("任务取消");
+    m_iActionClient_.cancelAllGoals();
+}
+
+void CActionTest::DoneCb(const actionlib::SimpleClientGoalState &state,
+                         const user_msgs::robot_pos_ctrlResultConstPtr &result) {
+    ROS_INFO("任务完成 : %s, %d", result->res.data(), m_nGoalNum_);
+    if (result->res == "fail") {
+        ros::shutdown();
+        return;
+    }
+    if (m_nGoalNum_ == -1) {
+        return;
+    }
+    m_nGoalNum_++;
+    if (m_nGoalNum_ > m_nMaxGoalNum_) {
+        m_nGoalNum_ = 0;
+    }
+}
+
+void CActionTest::ActiveCb() {
+    ROS_INFO("goal is active");
+    m_bIsActive_ = true;
+}
+
+void CActionTest::FeedbackCb(const user_msgs::robot_pos_ctrlFeedbackConstPtr &msg) {
+//    ROS_INFO("feedback is %d", msg->drived);
+}
+
+std::vector<user_msgs::NAV_POINT>
+CActionTest::CreatPath(std::vector<int> id, std::vector<int> dir, std::vector<int> type) {
+    std::vector<user_msgs::NAV_POINT> path;
+
+    for (int i = 0; i < id.size(); ++i) {
+        user_msgs::NAV_POINT point;
+        point.X = id[i];
+        point.yaw = dir[i];
+        point.type = type[i];
+        path.push_back(point);
+    }
+    return path;
+}
+
+user_msgs::robot_pos_ctrlGoal CActionTest::CreatGoal(std::vector<user_msgs::NAV_POINT> path) {
+    user_msgs::robot_pos_ctrlGoal goal;
+    goal.path = std::move(path);
+    return goal;
+}
+
+void CActionTest::KeyGetLoop() {
+    while (ros::ok()) {
+        auto code = scanKeyboard();
+        switch (code) {
+            case KEYCODE_q:
+                if (m_bIsActive_) {
+                    ROS_INFO("取消任务");
+                    m_iActionClient_.cancelAllGoals();
+                    m_nGoalNum_ = -1;
+                }
+                break;
+            case KEYCODE_SPACE:
+                if (m_bPause_) {
+                    ROS_INFO("继续执行任务");
+                } else {
+                    ROS_INFO("暂停执行任务");
+                }
+                break;
+            case KEYCODE_c:
+                ROS_INFO("开始任务新的任务");
+                m_nGoalNum_ = 0;
+                break;
+            default:
+                break;
+        }
+        ROS_INFO("键盘数值 : %d", scanKeyboard());
+    }
+}
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "action_test");
+    setlocale(LC_CTYPE, "zh_CN.utf8"); // 设置终端可以输出中文
+
+    ros::NodeHandle nh;
+    CActionTest cActionTest(nh);
+    return 0;
+}

src/main.cpp

@@ -0,0 +1,19 @@
+/*
+ * @Descripttion: main.cpp Project
+ * @version: Project v0.1
+ * @Author: lenotary
+ * @Date: 2021/4/21 下午1:26
+ * @LastEditor: lenotary
+ * @LastEditTime: 2021/4/21 下午1:26
+ */ 
+
+#include <iostream>
+#include "navigation.h"
+int main(int argc, char ** argv){
+    setlocale(LC_CTYPE,"zh_CN.utf8"); // 设置终端可以输出中文
+    ros::init(argc,argv,"navigation_node");
+    ros::NodeHandle nh;
+    CNavigation navigation(nh);
+    ros::spin();
+    return 0;
+}

src/move_action.cpp

@@ -0,0 +1,410 @@
+/*
+ * @Descripttion: move_action.cpp Project
+ * @version: Project v0.1
+ * @Author: lenotary
+ * @Date: 2021/4/21 下午1:24
+ * @LastEditor: lenotary
+ * @LastEditTime: 2021/4/21 下午1:24
+ */
+
+#include "move_action.h"
+
+/**
+ * @Title        :
+ * @MethodName   :  CMoveAction
+ * @param    in  :  ros::NodeHandle &nh
+ * @Return       :  null
+ * @Exception    :
+ * @Description  :  析构函数，初始化订阅
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午1:42
+ */
+
+CMoveAction::CMoveAction(ros::NodeHandle &nh) : m_iNh_(nh) {
+    m_iMagLineDataSub_ = nh.subscribe("/device/mag_line_data", 1, &CMoveAction::MagLineDataCallback, this);
+    m_iVelSub_ = nh.subscribe("/robot_vel", 1, &CMoveAction::VelCallback, this);
+    InitParam();
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  MagLineDataCallback
+ * @param    in  :  user_msgs::MagLineData::ConstPtr
+ * @Return       :  null
+ * @Exception    :
+ * @Description  :  磁条导航回调函数
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午1:45
+ */
+void CMoveAction::MagLineDataCallback(const user_msgs::MagLineData::ConstPtr &msg) {
+    m_iMagLineData_ = *msg;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  SonarCallback
+ * @param int    :  msg
+ * @Return       :
+ * @Exception    :
+ * @Description  :  订阅超声波数据
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午3:22
+ */
+void CMoveAction::SonarCallback(const std_msgs::Float32MultiArray::ConstPtr &msg) {
+    m_iSonarList_ = *msg;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  TracingPd
+ * @param   in   :  speed (设置速度)
+ * @param   out  :  twist （引用设置速度，反馈到调用者）
+ * @Return       :  bool
+ * @Exception    :
+ * @Description  :  PD控制巡线
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午1:46
+ */
+
+bool CMoveAction::TracingPd(double speed, geometry_msgs::Twist &twist) {
+    static double speedPrev{0};
+    static double speedAdd{0};
+    static double speedStart{0};
+    static double last_error{0};
+
+    if (speed != speedPrev) {
+        speedAdd = (speed - m_iVel_.linear.x) / 25;
+        speedStart = m_iVel_.linear.x;
+    }
+    speedPrev = speed;
+    speedStart += speedAdd;
+    if (speedAdd > 0) {
+        if (speedStart > speed) {
+            twist.linear.x = speed;
+        } else {
+            twist.linear.x = speedStart;
+        }
+    } else {
+        if (speedStart < speed) {
+            twist.linear.x = speed;
+
+        } else {
+            twist.linear.x = speedStart;
+
+        }
+    }
+
+    if (twist.linear.x > m_dMaxLineSpeed_) {
+        twist.linear.x = m_dMaxLineSpeed_;
+    } else if (twist.linear.x < -m_dMaxLineSpeed_) {
+        twist.linear.x = -m_dMaxLineSpeed_;
+    }
+
+    twist.angular.z = m_dDirP_ * m_iMagLineData_.bias + m_dDirD_ * (m_iMagLineData_.bias - last_error);
+    last_error = m_iMagLineData_.bias;
+    return false;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  Rotation
+ * @param  in      :  angularSpeed （设置,原地旋转速度）
+ * @param  out      :  twist (引用返回 速度)
+ * @Return       :  bool  （true 表示旋转完成）
+ * @Exception    :
+ * @Description  :  原地旋转
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午1:47
+ */
+bool CMoveAction::Rotation(double angularSpeed, geometry_msgs::Twist &twist) {
+    twist = geometry_msgs::Twist();
+    switch (m_eTurnAroundStep_) {
+        case e_STEP0:
+            m_eTurnAroundStep_ = e_STEP1;
+            break;
+        case e_STEP1:
+            twist.linear.x = 0;
+            twist.angular.z = angularSpeed;
+            if (m_iMagLineData_.lines == 0) {
+                m_eTurnAroundStep_ = e_STEP2;
+            }
+            break;
+        case e_STEP2:
+            twist.linear.x = 0;
+            twist.angular.z = angularSpeed;
+            if (m_iMagLineData_.lines != 0) {
+                m_eTurnAroundStep_ = e_STEP3;
+            }
+            break;
+        case e_STEP3:
+//            twist = tracingPd(0);
+            twist.linear.x = 0;
+            twist.angular.z = angularSpeed;
+            if (std::abs(m_iMagLineData_.bias) < 5) {
+                m_eTurnAroundStep_ = e_STEP_END;
+            }
+            break;
+        case e_STEP_END:
+            m_eTurnAroundStep_ = e_STEP0;
+            return true;
+            break;
+    }
+    if (twist.angular.z > m_dMaxAngularSpeed_) {
+        twist.angular.z = m_dMaxAngularSpeed_;
+    } else if (twist.angular.z < -m_dMaxAngularSpeed_) {
+        twist.angular.z = -m_dMaxAngularSpeed_;
+    }
+    return false;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  Clear
+ * @param null
+ * @Return       :  null
+ * @Exception    :
+ * @Description  :  清除变量
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午1:49
+ */
+void CMoveAction::Clear() {
+    m_eTurnAroundStep_ = e_STEP0;  // 原地旋转标志位清除
+    m_eMoveMode_ = e_MOVE_IDLE;
+    m_bChangeSpeeding_ = false;  // 方便保存速度平滑初始数据
+    m_dStopDistance_ = 0;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  VelCallback
+ * @param in     :  msg
+ * @Return       :  null
+ * @Exception    :
+ * @Description  :  订阅机器人，轮子速度
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午2:44
+ */
+void CMoveAction::VelCallback(const nav_msgs::Odometry::ConstPtr &msg) {
+    m_iVel_ = msg->twist.twist;
+    static double timePrev = ros::Time::now().toSec();
+    if(m_bIsStop_){
+        m_dStopDistance_ += m_iVel_.linear.x * ( ros::Time::now().toSec() - timePrev);
+    }else{
+        m_dStopDistance_ = 0;
+    }
+    timePrev =  ros::Time::now().toSec();
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  advance
+ * @param     in :  speed
+ * @param    out :  twist
+ * @Return       :  null
+ * @Exception    :
+ * @Description  :  机器人巡线向前，如果前面没有磁条则旋转一圈继续巡线
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午2:57
+ */
+bool CMoveAction::Advance(double speed, geometry_msgs::Twist &twist,bool useDistance) {
+
+    if(useDistance){
+        static double errorLast{0};
+        m_bIsStop_ = true;
+        double stopDistance = speed;
+        double error = stopDistance - m_dStopDistance_;
+        speed  =  m_dSpeedP_ * error + m_dSpeedD_ * (error - errorLast);
+        if(speed < 0.05){
+            speed = 0.05;
+        }
+    } else{
+        m_bIsStop_ = false;
+    }
+    if (m_iMagLineData_.lines > 0 and m_eTurnAroundStep_ == e_STEP0) {
+        TracingPd(speed, twist);
+    } else {
+        if (Rotation(m_dAngularSpeed_, twist)) {
+            m_eTurnAroundStep_ = e_STEP0;
+        }
+    }
+    return false;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  advanceStop
+ * @param in     :  speed 设置速度
+ * @param out    :  twist 反馈速度
+ * @Return       :
+ * @Exception    :
+ * @Description  :  巡线，没有磁条的时候，停止
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午3:13
+ */
+bool CMoveAction::AdvanceStop(double speed, geometry_msgs::Twist &twist) {
+    if (m_iMagLineData_.lines > 0) {
+        TracingPd(speed, twist);
+    } else {
+        twist = geometry_msgs::Twist();
+    }
+    return false;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  stopQuick
+ * @param out    :  twist
+ * @Return       :
+ * @Exception    :
+ * @Description  :  快速停止，机器人可能会发生振动
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午3:08
+ */
+bool CMoveAction::StopQuick(geometry_msgs::Twist &twist) {
+    twist = geometry_msgs::Twist();
+    return false;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  stopSmooth
+ * @param out    :  twist
+ * @Return       :  bool （ture 表示停止成功）
+ * @Exception    :
+ * @Description  :  平滑停止
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午3:10
+ */
+bool CMoveAction::StopSmooth(geometry_msgs::Twist &twist) {
+    if (std::abs(m_iVel_.linear.x) < 0.01) {
+        twist.linear.x = 0;
+        return true;
+    } else {
+        twist.linear.x = m_iVel_.linear.x - 0.02;
+    }
+    return false;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  SetMode
+ * @param in     :  mode
+ * @Return       :
+ * @Exception    :
+ * @Description  :  设置运动模式，前进，后退还是停止
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/23 下午1:21
+ */
+void CMoveAction::SetMode(MOVE_MODE mode) {
+    m_eMoveMode_ = mode;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  ChangeSpeedWithDistance
+ * @param in     :  speed 设定速度
+ * @param in     :  distance 设定距离
+ * @param out    :  twist 反馈速度
+ * @Return       :
+ * @Exception    :
+ * @Description  :  根据距离，修改速度达到设定值。主要功能速度平滑
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/23 下午1:31
+ */
+bool CMoveAction::ChangeSpeedWithDistance(double speed, double distance, geometry_msgs::Twist &twist) {
+    static double startSpeed{0};
+    static double lastSpeed{0};
+    static double pastDistance{0};
+    static double lastTime{0};
+    static double a{0};
+    static double b{0};   // 直线方程 y = ax + b
+
+    if (m_bChangeSpeeding_) { // 第一次运行修改速度，保持初始速度信息，让步计算差值。公式参数
+        startSpeed = m_iVel_.linear.x;
+        m_bChangeSpeeding_ = true;
+        lastTime = ros::Time::now().toSec();
+        lastSpeed = m_iVel_.linear.x;
+        a = (speed - startSpeed) / distance;
+        b = startSpeed;
+
+    } else {
+        pastDistance += lastSpeed * (ros::Time::now().toSec() - lastTime);
+        twist.linear.x = a * pastDistance + b;
+    }
+    if (std::abs(twist.linear.x - speed) < 0.03) {
+        m_bChangeSpeeding_ = false;
+        return true;
+    }
+    return false;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  SetRotationStep
+ * @param in     :  moveStep
+ * @Return       :  null
+ * @Exception    :
+ * @Description  :  设置旋转步骤，主要防止上次旋转没结束就重新开始
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/25 下午2:38
+ */
+bool CMoveAction::SetRotationStep(MOVE_STEP moveStep) {
+    m_eTurnAroundStep_ = moveStep;
+    return false;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  InitParam
+ * @param null
+ * @Return       :
+ * @Exception    :
+ * @Description  :  初始化对象相关参数
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/25 下午3:47
+ */
+void CMoveAction::InitParam() {
+    MyGetParam("/move_action_param/speed_p", m_dSpeedP_, 1.0);
+    MyGetParam("/move_action_param/speed_d", m_dSpeedD_, 1.0);
+    MyGetParam("/move_action_param/dir_p", m_dDirP_, 1.0);
+    MyGetParam("/move_action_param/dir_d", m_dDirD_, 1.0);
+    MyGetParam("/move_action_param/max_line_speed", m_dMaxLineSpeed_, 1.0);
+    MyGetParam("/move_action_param/max_angular_speed", m_dMaxAngularSpeed_, 1.0);
+    MyGetParam("/move_action_param/speed_angular", m_dAngularSpeed_, 1.0);
+    Clear();
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  MyGetParam
+ * @param in     :  paramName
+ * @param in     :  defaultValue
+ * @param out    :  param
+ * @Return       :  null
+ * @Exception    :
+ * @Description  :  判断参数服务器是否包含相关参数，不包含则输出告警信息。使用默认数据
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/25 下午3:48
+ */
+template<typename T>
+void CMoveAction::MyGetParam(std::string paramName, T &param, T defaultValue) {
+    if (not m_iNh_.hasParam(paramName))
+        ROS_WARN("[警告] 未找到参数 ： %s , 使用默认参数 %s", paramName.data(), std::to_string(defaultValue).data());
+    m_iNh_.param<T>(paramName, param, defaultValue);
+}

src/move_action.h

@@ -0,0 +1,96 @@
+/*
+ * @Descripttion: move_action.h Project
+ * @version: Project v0.1
+ * @Author: lenotary
+ * @Date: 2021/4/21 下午1:24
+ * @LastEditor: lenotary
+ * @LastEditTime: 2021/4/21 下午1:24
+ */
+
+#ifndef SRC_MOVE_ACTION_H
+#define SRC_MOVE_ACTION_H
+#include <iostream>
+#include <geometry_msgs/Twist.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+#include <ros/node_handle.h>
+#include <std_msgs/Float32MultiArray.h>
+#include "user_msgs/MagLineData.h"
+
+/**
+ * @Title:
+ * @ClassName: move_action.h .cpp
+ * @Description:
+ *
+ * @Copyright 2016-2021 sunwin - Powered By 研发中心
+ * @author: wangpengpeng
+ * @date:  2021-04-22 上午11:44
+ * @version V1.0
+ */
+
+enum MOVE_MODE{
+    e_MOVE_ADVANCE,
+    e_MOVE_SLOW_DOWN,
+    e_MOVE_STOP_SMOOTH,
+    e_MOVE_STOP_QUICK,
+    e_MOVE_IDLE,
+};
+/* 枚举类型： 表达运动动作需要的步骤，比如旋转需要分步骤来 */
+enum MOVE_STEP{
+    e_STEP0,
+    e_STEP1,
+    e_STEP2,
+    e_STEP3,
+    e_STEP_END
+};
+class CMoveAction {
+public:
+    explicit CMoveAction(ros::NodeHandle &nh);
+
+public:
+    void Clear();
+    void SetMode(MOVE_MODE mode);
+    bool TracingPd (double speed, geometry_msgs::Twist & twist);
+    bool Rotation (double angularSpeed, geometry_msgs::Twist & twist);
+    bool StopSmooth(geometry_msgs::Twist &twist);
+    bool StopQuick(geometry_msgs::Twist &twist);
+    bool Advance(double speed, geometry_msgs::Twist & twist,bool useDistance=false);
+    bool AdvanceStop(double speed, geometry_msgs::Twist & twist);
+    bool ChangeSpeedWithDistance(double speed, double distance, geometry_msgs::Twist &twist);
+    bool SetRotationStep(MOVE_STEP moveStep);
+
+private:
+    void InitParam();
+    void MagLineDataCallback(const user_msgs::MagLineData::ConstPtr &msg);
+    void SonarCallback(const std_msgs::Float32MultiArray::ConstPtr &msg);
+    void VelCallback(const nav_msgs::Odometry::ConstPtr &msg);
+    template <typename T>
+    void MyGetParam(std::string paramName, T &param, T defaultValue);
+private:
+
+
+    ros::NodeHandle m_iNh_;
+    ros::Subscriber m_iMagLineDataSub_;
+    ros::Subscriber m_iVelSub_;
+
+    double m_dSpeedP_{0}; //  speed PID P 参数
+    double m_dSpeedD_{0}; //  speed PID D 参数
+    double m_dDirP_{0};   // 方向控制 IPD P 参数
+    double m_dDirD_{0};   // 方向控制 PID D 参数
+    double m_dMaxLineSpeed_{0}; // 速度限制 最大线速度
+    double m_dMaxAngularSpeed_{0}; // 角速度限制
+    double m_dAngularSpeed_{0}; // 设置圆度旋转速度
+    double m_dChangeSpeedDistance_{0}; // 设置设置速度变化距离。通过距离来实现速度平滑
+    double m_dStopDistance_{0};
+    user_msgs::MagLineData m_iMagLineData_;
+    geometry_msgs::Twist m_iVel_;
+    std_msgs::Float32MultiArray m_iSonarList_;
+    MOVE_STEP m_eTurnAroundStep_{};
+
+    MOVE_MODE m_eMoveMode_{};
+    bool m_bChangeSpeeding_{false};
+    bool m_bIsStop_{false};
+};
+
+
+#endif //SRC_MOVE_ACTION_H

src/navigation.cpp

@@ -0,0 +1,452 @@
+/*
+ * @Descripttion: navigation.cpp Project
+ * @version: Project v0.1
+ * @Author: lenotary
+ * @Date: 2021/4/21 下午2:07
+ * @LastEditor: lenotary
+ * @LastEditTime: 2021/4/21 下午2:07
+ */
+
+
+
+
+#include "navigation.h"
+
+/**
+ * @Title        :
+ * @MethodName   :  CNavigation
+ * @param in     :  nh ros node 节点句柄
+ * @Return       :
+ * @Exception    :
+ * @Description  :  初始化 action server
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午3:45
+ */
+CNavigation::CNavigation(ros::NodeHandle &nh) : m_iNh_(nh), m_iActionServer_(nh, "/robot_pos_ctrl", [this](auto &&PH1) {
+    ExecuteCb(PH1);
+}, false) {
+    ROS_INFO("初始化 Navigation 对象 ... ");
+    m_iJoySpeed_ = geometry_msgs::Twist();
+    m_iActionServer_.registerPreemptCallback([this] { PreemptCb(); });
+    m_iRfidSub_ = nh.subscribe("/device/rfid_data", 1, &CNavigation::RfidCallback, this);
+    m_iCoproSwitchSub_ = nh.subscribe("/copro_switch", 10, &CNavigation::CoproSwitchCallback, this);
+    m_iSpecialControlSub_ = nh.subscribe("/task_special_ctrl", 10, &CNavigation::SpecialControlCallback, this);
+    m_iJoySub_ = nh.subscribe("/label_vel", 10, &CNavigation::LabelVelCallback, this);
+    m_iModeSub_ = nh.subscribe("/sys/mode_switch", 10, &CNavigation::ModeCallback, this);
+
+    m_iVelPub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
+    m_iMagLineSelectPub_ = nh.advertise<user_msgs::MagLineSelect>("/device/mag_line_select", 1);
+    m_iRobotPosePub_ = nh.advertise<user_msgs::robot_pos>("/robot_pos", 1);
+    m_pfnMoveAction_ = new CMoveAction(nh);
+    InitParam();
+    m_iActionServer_.start();
+    ROS_INFO("初始化成功");
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  ExecuteCb
+ * @param in     :  goal
+ * @Return       :  null
+ * @Exception    :
+ * @Description  :  action 服务回调函数，当有任务从客户端发过来的时候，就会调用这个函数。任务结束，则跳出这个函数
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午3:45
+ *
+ *
+ *
+ */
+void CNavigation::ExecuteCb(const user_msgs::robot_pos_ctrlGoalConstPtr &goal) {
+    m_bActionActive = true;
+    user_msgs::robot_pos_ctrlResult result;
+    Clear();
+    ROS_INFO("[导航任务开始] ++++++++++++++++++++++++++++++");
+    ROS_INFO("接受到新任务, 路径大小 %ld", goal->path.size());
+    m_vPath_ = goal->path;
+    //打印路径点信息，把ID保存成列表，方便后面判断点是否在路径里
+    for (int i = 0; i < m_vPath_.size(); i++) {
+        auto point = goal->path[i];
+        m_vIdList_.push_back(static_cast<int>(point.X));
+        ROS_INFO("点位信息 Index : %d , ID：%d , 方向 %d , 类型 %d", i, static_cast<int>(point.X), static_cast<int>(point.yaw),
+                 point.type);
+    }
+    // 计算终点在路径中出现的次数，主要为了有的路径终点出现两次情况，比如需要掉头回到终点。或者循环多次任务等
+    for (int i = 1; i < m_vPath_.size(); ++i) {
+        if (m_vPath_[i].X == m_vPath_.back().X) {
+            m_nGoalTime_++;
+        }
+    }
+    m_iLineSelect_.select = static_cast<int>(m_vPath_[m_nIdIndex_].yaw);
+    ROS_INFO("[信息] 选择路线 %d %f", m_iLineSelect_.select,m_vPath_[m_nIdIndex_].X);
+    ROS_INFO("终点需要经过的次数 %d", m_nGoalTime_);
+//    for (auto point: m_vPath_) {
+//        if (point.X == m_vPath_.back().X) {
+//            m_nGoalTime_++;
+//        }
+//    }
+    geometry_msgs::Twist twist;
+    user_msgs::robot_pos_ctrlFeedback feedback;
+    feedback.drived = 1;
+    ros::Rate rate(50);
+    while (m_bActionActive and ros::ok()) {
+        switch (m_eActionState) {
+            case e_ACTION_IDLE:
+                ROS_INFO("[信息] 开始导航");
+                m_eActionState = e_ACTION_RUNNING;
+                break;
+            case e_ACTION_RUNNING:
+                m_pfnMoveAction_->Advance(m_dNavSpeedMid_, twist);
+                break;
+            case e_ACTION_PARKING:
+
+                m_pfnMoveAction_->Advance(0.15, twist, true);
+                break;
+            case e_ACTION_ROTATION:
+                if (m_pfnMoveAction_->Rotation(m_dRotationSpeedMax_, twist)) {
+                    m_eActionState = e_ACTION_RUNNING;
+                    ROS_INFO("[信息] 机器人原地旋转结束");
+                }
+                break;
+            case e_ACTION_END:
+                m_pfnMoveAction_->StopQuick(twist);
+                m_eActionState = e_ACTION_IDLE;
+                result.res = "ok";
+                m_iActionServer_.setSucceeded(result);
+                ROS_INFO("[导航]  导航任务完成 ");
+                m_bActionActive = false; //任务完成
+                break;
+            case e_ACTION_POINT_ERROR:
+                if (m_pfnMoveAction_->StopSmooth(twist)) {
+                    result.res = "fail";
+                    m_iActionServer_.setSucceeded(result);
+                    ROS_ERROR("[错误] 机器人停止");
+                    m_eActionState = e_ACTION_IDLE;
+                    m_bActionActive = false; // 任务完成；
+                }
+                break;
+        }
+        if(m_bIsObstacle_){
+            twist = geometry_msgs::Twist();
+        }
+        m_iActionServer_.publishFeedback(feedback);
+        m_iMagLineSelectPub_.publish(m_iLineSelect_);
+        m_iVelPub_.publish(twist);
+        rate.sleep();
+    }
+    ROS_INFO("[导航任务结束] ------------------------------");
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  PreemptCb
+ * @param null
+ * @Return       :
+ * @Exception    :
+ * @Description  :  任务执行中，如果任务被中断，则任务停止清除标志为
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午3:47
+ */
+void CNavigation::PreemptCb() {
+    ROS_WARN("任务被中断");
+    geometry_msgs::Twist twist;
+    while (!m_pfnMoveAction_->StopSmooth(twist)) {
+        ros::Duration(0.05).sleep();
+        m_iVelPub_.publish(twist);
+    }
+    m_iVelPub_.publish(twist);
+    m_pfnMoveAction_->Clear();
+    m_bActionActive = false;
+    user_msgs::robot_pos_ctrlResult result;
+    result.res = "fail";
+    m_iActionServer_.setSucceeded(result);
+    ROS_WARN("机器人停止");
+
+
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  RfidCallback
+ * @param in     :  msg
+ * @Return       :
+ * @Exception    :
+ * @Description  :  订阅 RFID信息，逻辑处理，根据当前RFID的信息，判断点的类型和下一个目标点需要行走的方向。
+ *                  判断是否需要在本次目标点停下。还是旋转。
+ *
+ *
+ *
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午5:17
+ */
+void CNavigation::RfidCallback(const user_msgs::RfidMsg_<std::allocator<void>>::ConstPtr &msg) {
+    static int lastId{0};
+    ROS_WARN("读取到RFID信息 : %d", msg->id);
+    user_msgs::robot_pos robotPosMsg;
+    robotPosMsg.x = msg->id*1000;
+    m_iRobotPosePub_.publish(robotPosMsg);
+    if (m_eActionState == e_ACTION_IDLE)
+        return;
+    lastId = m_nCurrentId_;
+    m_nCurrentId_ = msg->id;
+    
+    m_vPastId_.push_back(m_nCurrentId_);
+    ROS_INFO("点位索引 : %d", m_nIdIndex_);
+    ROS_INFO("经过的路径有 : %s", VectorToStr(m_vPastId_).data());
+    if(GetPointType(m_nCurrentId_) == e_ROTATION_POINT and lastId == m_nCurrentId_){
+        ROS_INFO("原地旋转");
+        return;
+    }
+    if (m_nIdIndex_ > m_vPath_.size() - 1) {
+        ROS_INFO("[错误] 路径点错误，路径过多。机器人停止");
+        m_eActionState = e_ACTION_POINT_ERROR;
+        return;
+    }
+    if (m_nCurrentId_ != m_vPath_[m_nIdIndex_].X) {
+        ROS_INFO("[错误] 路径点错误，错误点。机器人停止 %d %f", m_nCurrentId_, m_vPath_[m_nIdIndex_].X);
+        m_eActionState = e_ACTION_POINT_ERROR;
+        return;
+    }
+    m_nIdIndex_ += 1;
+    if(m_nIdIndex_ < m_vPath_.size()){
+        m_iLineSelect_.select = static_cast<int>(m_vPath_[m_nIdIndex_].yaw);
+        ROS_INFO("[信息] 选择路线 %d %f", m_iLineSelect_.select,m_vPath_[m_nIdIndex_].X);
+    }else{
+        ROS_INFO("最后一个点");
+    }
+    if (not IsInclude(m_vIdList_, m_nCurrentId_)) {
+        m_eActionState = e_ACTION_POINT_ERROR;
+        ROS_INFO("[错误] 路径点错误，路径不包含当前点。机器人停止");
+    }
+    //判断旋转点是否是第一次经过，由于机器人旋转过后会再经过一次旋转点。所以第二次不会旋转。默认机器人只会经过一次旋转点
+    if (GetPointType(m_nCurrentId_) == e_ROTATION_POINT) {
+        m_eActionState = e_ACTION_ROTATION;
+        ROS_INFO("到达旋转点，减速准备原地旋转");
+    } else if (m_nCurrentId_ == static_cast<int>(m_vPath_.back().X) or static_cast<int>(m_vPath_.back().X) == -1) {
+        int pastGoalTimes{0}; //计算机器经过终点次数。
+        for (auto id : m_vPastId_) {
+            if (id == m_vPath_.back().X) {
+                pastGoalTimes++;
+            }
+        }
+        ROS_INFO("机器人经过终点次数 ： %d", pastGoalTimes);
+        if (pastGoalTimes == m_nGoalTime_) {
+            ROS_INFO("到达终点,机器人减速停止");
+            m_eActionState = e_ACTION_PARKING;
+        }
+    }
+
+
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  CoproSwitchCallback
+ * @param in     :  msg
+ * @Return       :  null
+ * @Exception    :
+ * @Description  :  订阅激光测距传感器，主要是检测下面RFID反光板，检测到反光板表示到达目标点需要立即停止。
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/25 上午10:14
+ */
+void CNavigation::CoproSwitchCallback(const user_msgs::CoproSwitch::ConstPtr &msg) {
+    if(msg->cmd == user_msgs::CoproSwitch::CMD_PHOTO_SWITCH){
+        if (m_eActionState == e_ACTION_PARKING) {
+            if (GetPointType(m_nCurrentId_) == e_ROTATION_POINT) {
+                m_eActionState = e_ACTION_ROTATION;
+                m_pfnMoveAction_->SetRotationStep(e_STEP0);
+                ROS_INFO("开始原地旋转");
+            } else if (GetPointType(m_nCurrentId_) == e_STOP_POINT or GetPointType(m_nCurrentId_) == e_START_POINT or GetPointType(m_nCurrentId_) == e_CHARGE_POINT) {
+                m_eActionState = e_ACTION_END;
+                ROS_INFO("机器人到达目标点，停止");
+            }
+        }
+        m_bIsObstacle_ = false;
+    }else if(msg->cmd == user_msgs::CoproSwitch::CMD_OBSTACLE_LASER){
+        if(msg->param == 1){
+            m_bIsObstacle_ = true;
+            ROS_WARN("[警告] 前方有障碍物");
+        }else{
+            m_bIsObstacle_ = false;
+            ROS_WARN("[警告] 障碍物消失");
+        }
+
+    }else{
+        m_bIsObstacle_ = false;
+    }
+
+}
+void CNavigation::SpecialControlCallback(const user_msgs::special_ctrl::ConstPtr &msg) {
+    if(msg->stop){
+        ROS_WARN("[警告]紧急停止");
+        PreemptCb();
+    }
+    if(msg->release){
+        ROS_WARN("[警告]取消急停");
+    }
+}
+void CNavigation::LabelVelCallback(const geometry_msgs::Twist::ConstPtr &msg) {
+    if(!m_bActionActive){
+        geometry_msgs::Twist twist;
+        if(msg->angular.z > 0.05){
+            m_iLineSelect_.select = 1;
+        }else if(msg->angular.z < -0.05){
+            m_iLineSelect_.select = 3;
+        } else{
+            m_iLineSelect_.select = 2;
+        }
+        if(std::abs(msg->linear.x) > 0.05){
+            m_pfnMoveAction_->Advance(msg->linear.x,twist);
+        }else{
+            twist.angular.z = msg->angular.z;
+        }
+        m_iMagLineSelectPub_.publish(m_iLineSelect_);
+        m_iVelPub_.publish(twist);
+
+
+
+
+
+
+
+
+    }
+
+}void CNavigation::ModeCallback(const user_msgs::SysModeSwitch::ConstPtr &msg) {
+    static user_msgs::SysModeSwitch sysModeSwitchPrev;
+    if(msg->mode != sysModeSwitchPrev.mode){
+        ROS_INFO("机器人模式 %d",msg->mode);
+    }
+    m_iMode_ = *msg;
+}
+/**
+ * @Title        :
+ * @MethodName   :  Clear
+ * @param null
+ * @Return       :
+ * @Exception    :
+ * @Description  :  清除导航需要的相关信息和标志位，每次导航结束的时候清除一下。
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/22 下午5:21
+ */
+void CNavigation::Clear() {
+    ROS_INFO("清除上次Action, 相关缓存");
+    m_vPastId_.clear();
+    m_vPath_.clear();
+    m_pfnMoveAction_->Clear();
+    m_eActionState = e_ACTION_IDLE;
+    m_nIdIndex_ = 1;
+    m_nGoalTime_ = 0;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  VectorToStr
+ * @param in     :  list
+ * @Return       :
+ * @Exception    :
+ * @Description  :  把数组内数据变成字符串，方便打印
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/23 上午11:38
+ */
+
+std::string CNavigation::VectorToStr(std::vector<int> list) {
+    std::string str;
+
+    for (auto data:list) {
+        str += "[" + std::to_string(data) + "] ";
+    }
+    return str;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  IsInclude
+ * @param in     :  list 数组
+ * @param in     :  key     需要判断的数据
+ * @Return       :  true 表示数组内保护 相应元素
+ * @Exception    :
+ * @Description  :  判断数组是否包含某元素
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/23 上午11:39
+ */
+bool CNavigation::IsInclude(std::vector<int> list, int key) {
+    if (std::find(list.begin(), list.end(), key) != list.end()) {
+        return true;
+    }
+    return false;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  GetPointType
+ * @param in     :  id
+ * @Return       :  pointType
+ * @Exception    :
+ * @Description  :  根据id号，检索相应的点的类型
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/23 上午11:42
+ */
+int CNavigation::GetPointType(int id) {
+
+    for (auto point : m_vPath_) {
+        if (static_cast<int>(point.X) == id) {
+            return point.type;
+        }
+    }
+    return e_OTHER_POINT;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  InitParam
+ * @param null
+ * @Return       :
+ * @Exception    :
+ * @Description  :  初始化对象相关参数
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/25 下午3:47
+ */
+void CNavigation::InitParam() {
+    MyGetParam("/nav_param/nav_speed_max", m_dNavSpeedMax_, 0.8);
+    MyGetParam("/nav_param/nav_speed_mid", m_dNavSpeedMid_, 0.5);
+    MyGetParam("/nav_param/nav_speed_min", m_dNavSpeedMin_, 0.2);
+    MyGetParam("/nav_param/speed_angular", m_dRotationSpeedMax_, 0.2);
+    m_iLineSelect_.select = 1;
+    m_eActionState = e_ACTION_IDLE;
+}
+
+/**
+ * @Title        :
+ * @MethodName   :  MyGetParam
+ * @param in     :  paramName
+ *
+ *
+ * @param in     :  defaultValue
+ * @param out    :  param
+ * @Return       :  null
+ * @Exception    :
+ * @Description  :  判断参数服务器是否包含相关参数，不包含则输出告警信息。使用默认数据
+ *
+ * @author       :  王朋朋
+ * @date         :  2021/4/25 下午3:48
+ */
+template<typename T>
+void CNavigation::MyGetParam(std::string paramName, T &param, T defaultValue) {
+    if (not m_iNh_.hasParam(paramName))
+        ROS_WARN("[警告] 未找到参数 ： %s , 使用默认参数 %s", paramName.data(), std::to_string(defaultValue).data());
+    m_iNh_.param<T>(paramName, param, defaultValue);
+}
+
+
+
+

src/navigation.h

@@ -0,0 +1,101 @@
+/*
+ * @Descripttion: navigation.h Project
+ * @version: Project v0.1
+ * @Author: lenotary
+ * @Date: 2021/4/21 下午2:07
+ * @LastEditor: lenotary
+ * @LastEditTime: 2021/4/21 下午2:07
+ */
+
+#ifndef SRC_NAVIGATION_H
+#define SRC_NAVIGATION_H
+
+#include <actionlib/server/simple_action_server.h>
+#include "user_msgs/robot_pos_ctrlAction.h"
+#include "user_msgs/NAV_POINT.h"
+#include "user_msgs/RfidMsg.h"
+#include "user_msgs/CoproSwitch.h"
+#include "user_msgs/MagLineSelect.h"
+#include "user_msgs/robot_pos.h"
+#include "user_msgs/special_ctrl.h"
+#include "user_msgs/SysModeSwitch.h"
+#include "move_action.h"
+typedef actionlib::SimpleActionServer<user_msgs::robot_pos_ctrlAction> ActionServer;
+enum POINT_TYPE{
+    e_START_POINT,
+    e_CHARGE_POINT,
+    e_INTERSECTION_POINT,
+    e_STOP_POINT,
+    e_END_POINT,
+    e_ROTATION_POINT,
+    e_OTHER_POINT
+};
+enum ACTION_STATE{
+    e_ACTION_IDLE,
+    e_ACTION_RUNNING,
+    e_ACTION_ROTATION,
+    e_ACTION_PARKING,
+    e_ACTION_END,
+    e_ACTION_POINT_ERROR,
+};
+class CNavigation {
+public:
+    explicit CNavigation(ros::NodeHandle &nh);
+    ~CNavigation() = default;
+
+private:
+    void ExecuteCb(const user_msgs::robot_pos_ctrlGoalConstPtr& goal);
+    void PreemptCb();
+    void RfidCallback(const user_msgs::RfidMsg::ConstPtr &msg);
+    void CoproSwitchCallback(const user_msgs::CoproSwitch::ConstPtr &msg);
+    void SpecialControlCallback(const user_msgs::special_ctrl::ConstPtr &msg);
+    void LabelVelCallback(const geometry_msgs::Twist::ConstPtr &msg);
+    void ModeCallback(const user_msgs::SysModeSwitch::ConstPtr &msg);
+    void Clear();
+    void InitParam();
+    template <typename T>
+    void MyGetParam(std::string paramName, T &param, T defaultValue);
+    std::string VectorToStr(std::vector<int>  list);
+    bool IsInclude(std::vector<int>  list, int key);
+    int GetPointType(int id);
+private:
+    CMoveAction *m_pfnMoveAction_;
+    ros::NodeHandle m_iNh_;
+    ros::Publisher m_iVelPub_;  //订阅 发布机器人速度信息
+    ros::Publisher m_iMagLineSelectPub_; //发布 磁条选择信息
+    ros::Publisher m_iRobotPosePub_; //发布 机器人位置信息
+    ros::Subscriber m_iRfidSub_;      // 订阅rfid 信息
+    ros::Subscriber m_iCoproSwitchSub_;      // 订阅rfid 信息
+    ros::Subscriber m_iSpecialControlSub_;      // 订阅rfid 信息
+    ros::Subscriber m_iJoySub_;      // 订阅Joy 信息
+    ros::Subscriber m_iLabelSub_;      // 订阅Joy 信息
+    ros::Subscriber m_iModeSub_;      // 订阅Joy 信息
+
+    geometry_msgs::Twist m_iJoySpeed_;
+    ActionServer m_iActionServer_;  // 动作服务器
+    std::vector<user_msgs::NAV_POINT> m_vPath_;
+    user_msgs::MagLineSelect m_iLineSelect_;
+    user_msgs::SysModeSwitch m_iMode_;
+    //设置3档导航速度
+    double m_dNavSpeedMax_{0};  //导航时的速度  大
+    double m_dNavSpeedMid_{0};  //导航时的速度  中
+    double m_dNavSpeedMin_{0};  //导航时的速度  小
+    double m_dRotationSpeedMax_{0};  //导航时的速度  小
+
+
+    //RFID 相关变量
+    std::vector<int> m_vPastId_; //经过的ID
+    std::vector<int> m_vIdList_; //路径上的RFID，主要做判断机器人移动路径是否超过路径
+    int m_nCurrentId_{0};
+
+    //任务相关变量
+    int m_nGoalTime_{0}; // 判断路径上终点出现次数
+    int m_nIdIndex_{0}; // 判断路径上终点出现次数
+    bool m_bParking{false}; // 判断是不是停止
+    bool m_bActionActive{true};
+    bool m_bIsObstacle_{false};
+    ACTION_STATE m_eActionState{};
+};
+
+
+#endif //SRC_NAVIGATION_H