navigation adapter implemented. Receives requests and handles service requests to the navigation stack using priorities.

Author: globeofgeek

arc_behaviour/CMakeLists.txt

@@ -192,18 +192,18 @@ include_directories(
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
-add_executable(arc_base src/arc_base_node.cpp src/ArcBase.cpp src/ArcBase.h)
+add_executable(arc_base src/arc_base_node.cpp src/ArcBase.cpp include/ArcBase.h)
 
 target_link_libraries(arc_base
   ${catkin_LIBRARIES}
 )
 
 #PERCEPTUAL SCHEMAS
-add_executable(detect_marker_ps src/DetectMarkerPS.cpp src/DetectMarkerPS.h src/detect_marker_ps_node.cpp)
-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)
+add_executable(detect_marker_ps src/DetectMarkerPS.cpp include/DetectMarkerPS.h src/detect_marker_ps_node.cpp)
+add_executable(detect_robot_ps src/detect_robot_ps_node.cpp src/DetectRobotPS.cpp include/DetectRobotPS.h)
+add_executable(detect_debris_ps src/detect_debris_ps_node.cpp src/DetectDebrisPS.cpp include/DetectDebrisPS.h)
+add_executable(detect_victim_ps src/detect_victim_ps_node.cpp src/DetectVictimPS.cpp include/DetectVictimPS.h)
+add_executable(navigation_adapter src/NavigationAdapter.cpp include/NavigationAdapter.h src/navigation_adapter_node.cpp)
 
 target_link_libraries(detect_victim_ps ${catkin_LIBRARIES} )
 target_link_libraries(detect_marker_ps ${catkin_LIBRARIES} )

arc_behaviour/src/ArcBase.h renamed to arc_behaviour/include/ArcBase.h

@@ -22,6 +22,7 @@ typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseCl
 namespace arc_behaviour {
     class NavigationAdapter{
     private:
+        const int DEFAULT_NAVIGATION_PRIORITY = 0;
         ros::NodeHandle global_handle;
 
         ros::NodeHandle local_handle;
@@ -40,6 +41,17 @@ namespace arc_behaviour {
          * Whether or not the navigation stack is currently being used to reach a goal.
          */
         bool goal_active;
+        /**
+         * When goal is active, we have some priority that was specified with the navigation request.
+         * If any further requests are made to change the navigation, the priority must be higher than the value specified here.
+         */
+        int current_nav_priority;
+
+        /**
+         * Process navigation request and send it to be handled by navigation stack.
+         * @param req: The accepted navigation request.
+         */
+        void sendGoal(arc_msgs::NavigationRequest::Request &req);
 
     public:
         NavigationAdapter();

arc_behaviour/src/DetectDebrisPS.h renamed to arc_behaviour/include/DetectDebrisPS.h

@@ -1,4 +1,4 @@
-#include "ArcBase.h"
+#include "../include/ArcBase.h"
 
 using namespace arc_behaviour;
 

arc_behaviour/src/DetectMarkerPS.h renamed to arc_behaviour/include/DetectMarkerPS.h

@@ -1,6 +1,6 @@
 #include <rosconsole/macros_generated.h>
 #include <ros/ros.h>
-#include "DetectDebrisPS.h"
+#include "../include/DetectDebrisPS.h"
 #include "arc_msgs/DetectedDebris.h"
 #include "arc_msgs/Debris.h"
 #include "marker_msgs/MarkerDetection.h"

arc_behaviour/src/DetectRobotPS.h renamed to arc_behaviour/include/DetectRobotPS.h

@@ -1,6 +1,6 @@
 #include <rosconsole/macros_generated.h>
 #include <ros/ros.h>
-#include "DetectMarkerPS.h"
+#include "../include/DetectMarkerPS.h"
 #include "marker_msgs/MarkerDetection.h"
 #include "std_msgs/Bool.h"