basic navigation adapter setup. Can receive custom navigation request, and respond with yes/no indicating if the navigation stack was able to reach desired goal. Also did loadtesting on ROS service architecture. It can handle frequent service requests, but move_base cannot, so I will use a priority system to ensure only prioritised services are sent to stack.

Author: globeofgeek

arc_behaviour/CMakeLists.txt

@@ -10,6 +10,9 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
   marker_msgs
   geometry_msgs
+  actionlib
+  move_base_msgs
+  actionlib_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -200,12 +203,15 @@ add_executable(detect_marker_ps src/DetectMarkerPS.cpp src/DetectMarkerPS.h src/
 add_executable(detect_robot_ps src/detect_robot_ps_node.cpp src/DetectRobotPS.cpp src/DetectRobotPS.h)
 add_executable(detect_debris_ps src/detect_debris_ps_node.cpp src/DetectDebrisPS.cpp src/DetectDebrisPS.h)
 add_executable(detect_victim_ps src/detect_victim_ps_node.cpp src/DetectVictimPS.cpp src/DetectVictimPS.h)
+add_executable(navigation_adapter src/NavigationAdapter.cpp src/NavigationAdapter.h src/navigation_adapter_node.cpp)
 
 target_link_libraries(detect_victim_ps ${catkin_LIBRARIES} )
 target_link_libraries(detect_marker_ps ${catkin_LIBRARIES} )
 target_link_libraries(detect_robot_ps ${catkin_LIBRARIES} )
 target_link_libraries(detect_debris_ps ${catkin_LIBRARIES} )
+target_link_libraries(navigation_adapter ${catkin_LIBRARIES} )
 
-add_dependencies(detect_victim_ps arc_msgs_generate_messages_cpp)
-add_dependencies(detect_debris_ps arc_msgs_generate_messages_cpp)
-#add_dependencies(detect_robot_ps arc_msgs_generate_messages_cpp)
+add_dependencies(detect_debris_ps marker_msgs_generate_messages_cpp arc_msgs_generate_messages_cpp)
+add_dependencies(detect_victim_ps marker_msgs_generate_messages_cpp arc_msgs_generate_messages_cpp)
+add_dependencies(detect_robot_ps marker_msgs_generate_messages_cpp arc_msgs_generate_messages_cpp)
+add_dependencies(navigation_adapter arc_msgs_generate_messages_cpp)

arc_behaviour/launch/default.launch

@@ -12,4 +12,8 @@
     <node name="detect_debris_ps" pkg="arc_behaviour" type="detect_debris_ps">
         <param name="max_range" value="6"/>
     </node>
+
+    <node name="navigation_adapter" pkg="arc_behaviour" type="navigation_adapter">
+    </node>
+
 </launch>

arc_behaviour/package.xml

@@ -41,7 +41,9 @@
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscpp</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
   <run_depend>roscpp</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>arc_msgs</build_depend>

arc_behaviour/src/NavigationAdapter.cpp

@@ -0,0 +1,67 @@
+#include "NavigationAdapter.h"
+#include <actionlib/client/simple_action_client.h>
+#include "arc_msgs/NavigationRequest.h"
+#include "std_srvs/Empty.h"
+
+using namespace arc_behaviour;
+
+NavigationAdapter::NavigationAdapter() {
+    ros::NodeHandle nh;
+    this->global_handle = nh;
+    ros::NodeHandle local_handle("navigation_adapter");
+    this->local_handle = local_handle;
+    ROS_INFO("Setting up navigation adapter..");
+    this->move_to_goal_server = local_handle.advertiseService("move_to_goal",&NavigationAdapter::move_to_goal_request_cb, this);
+    this->move_client = new MoveBaseClient("move_base", true);
+    //wait for the action server to come up
+    while(!move_client->waitForServer(ros::Duration(5.0))) { //TODO: Better checking to ensure action server is actually up. If it's not, we need to let user know.
+        ROS_INFO("Waiting for the move_base action server to come up");
+    }
+    ROS_INFO("Found move_base action server!");
+}
+
+void NavigationAdapter::move_to_goal_result_cb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result) {
+    if (state == actionlib::SimpleClientGoalState::ABORTED){
+        ROS_INFO("Failed to move");
+    }else if(state == actionlib::SimpleClientGoalState::SUCCEEDED){
+        ROS_INFO("successfully moved to goal!!!");
+    }
+
+    //either way, we are done navigating (until another request is sent)
+    this->goal_active = false;
+}
+
+bool NavigationAdapter::move_to_goal_request_cb(arc_msgs::NavigationRequest::Request &req, arc_msgs::NavigationRequest::Response &res) {
+    ROS_INFO("Received request to move to goal (%d, %d) with priority %d", req.pose.position.x, req.pose.position.y, req.priority);
+
+    move_base_msgs::MoveBaseGoal goal;
+    if(this->goal_active) {
+    } else {
+        //If no goal is currently active, we can send request to move_base
+        //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 = req.pose.position.x;
+        goal.target_pose.pose.orientation.w = 1.0;
+
+        ROS_INFO("Sending navigation goal");
+        this->move_client->sendGoal(goal, boost::bind(&NavigationAdapter::move_to_goal_result_cb, this,_1,_2));
+        return true;
+    }
+}
+
+void NavigationAdapter::run() {
+    ros::Rate r(10); //TODO: Make this a parameter, just get rid of magic numbers.
+
+    while(ros::ok()) {
+
+        /*if(this->move_client.getState() == actionlib::SimpleClientGoalState::ACTIVE) {
+            ROS_INFO("Navigation goal is active.");
+        } else {
+            ROS_INFO("navigation is not active");
+        }*/
+        ros::spinOnce();
+        r.sleep();
+    }
+}

arc_behaviour/src/NavigationAdapter.h

@@ -0,0 +1,64 @@
+/**
+* CLASS: NavigationAdaptor
+* DATE: 16/02/17
+* AUTHOR: Kyle Morris
+* DESCRIPTION: Provides adapted interface between arc_base and ros navigation stack.
+* Allow for filtering navigation requests based on priority. For example... when randomly wandering,
+ * multiple navigation requests may be sent to the navigation stack. When a request to move to a robot is sent,
+ * this will have a higher priority, and will not be interrupted by other requests. This ensures important navigation is completed.
+*/
+
+#ifndef PROJECT_NAVIGATIONADAPTER_H
+#define PROJECT_NAVIGATIONADAPTER_H
+
+#include <ros/ros.h>
+#include "std_srvs/Empty.h"
+#include <geometry_msgs/Pose.h>
+#include "arc_msgs/NavigationRequest.h"
+#include <move_base_msgs/MoveBaseAction.h>
+#include <actionlib/client/simple_action_client.h>
+
+typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
+namespace arc_behaviour {
+    class NavigationAdapter{
+    private:
+        ros::NodeHandle global_handle;
+
+        ros::NodeHandle local_handle;
+
+        /*
+         * Any requests to move to some location will be checked here before calling upon navigation stack.
+         */
+        ros::ServiceServer move_to_goal_server;
+
+        /**
+         * Our communication with the navigation stack.
+         */
+        MoveBaseClient *move_client;
+
+        /**
+         * Whether or not the navigation stack is currently being used to reach a goal.
+         */
+        bool goal_active;
+
+    public:
+        NavigationAdapter();
+
+        /**
+         * Move to navigation target location.
+         */
+        bool move_to_goal_request_cb(arc_msgs::NavigationRequest::Request &req, arc_msgs::NavigationRequest::Response &res);
+
+        /**
+         * When move_base is done, it will send a callback to signal the result of navigation. This may be either when the navigation is completed
+         * succesfully or it fails to navigate.
+         */
+        void move_to_goal_result_cb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
+
+        //run the main loop of node.
+        void run();
+    };
+    //TODO: make sure getters/setters are set for all classes in behaviour module
+}
+
+#endif //PROJECT_NAVIGATIONADAPTOR_H

arc_behaviour/src/detect_marker_ps_node.cpp

@@ -3,7 +3,6 @@
 
 int main(int argc, char **argv) {
     ros::init(argc, argv, "detect_marker_ps_node");
-
     arc_behaviour::DetectMarkerPS detector;
     detector.run();
 

arc_behaviour/src/navigation_adapter_node.cpp

@@ -0,0 +1,9 @@
+#include "ros/ros.h"
+#include "NavigationAdapter.h"
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "navigation_adaptor");
+    arc_behaviour::NavigationAdapter adapter;
+    adapter.run();
+    return 0;
+}

arc_msgs/CMakeLists.txt

@@ -56,11 +56,10 @@ add_message_files(
 )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+   FILES
+   NavigationRequest.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(

arc_msgs/srv/NavigationRequest.srv

@@ -0,0 +1,4 @@
+int32 priority 	#how important is this request? The higher, the better.
+geometry_msgs/Pose pose	#where to move to
+---
+bool result	#true if the request was accepted. False if rejected.