Testing behaviour. Ability to control pioneer P3DX using navigation stack. Creating demo worlds and preparing to make standard test/demo launch files.

Author: globeofgeek

arc_behaviour/CMakeLists.txt

@@ -193,7 +193,7 @@ 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 include/ArcBase.h src/RandomWanderMS.cpp src/RandomWanderMS.h)
+add_executable(arc_base src/arc_base_node.cpp src/ArcBase.cpp include/ArcBase.h src/RandomWanderMS.cpp src/RandomWanderMS.h src/MotorSchema.h)
 
 target_link_libraries(arc_base
   ${catkin_LIBRARIES}
@@ -205,7 +205,7 @@ add_executable(detect_robot_ps src/detect_robot_ps_node.cpp src/DetectRobotPS.cp
 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)
-add_executable(random_wander_ms src/RandomWanderMS.cpp src/RandomWanderMS.h src/random_wander_ms_node.cpp)
+add_executable(random_wander_ms src/RandomWanderMS.cpp src/RandomWanderMS.h src/random_wander_ms_node.cpp src/MotorSchema.h)
 
 target_link_libraries(detect_victim_ps ${catkin_LIBRARIES} )
 target_link_libraries(detect_marker_ps ${catkin_LIBRARIES} )

arc_behaviour/include/ArcBase.h

@@ -3,11 +3,11 @@
 * DATE: 13/02/17
 * AUTHOR: Kyle Morris
 * DESCRIPTION: Central entry point for behaviour in arc_ros.
- * Manages motor/perceptual schemas and receives motor schema activation vectors and adds them together,
- * then publishes final result on cmd_vel.
 */
 #include "ros/ros.h"
 #include "geometry_msgs/TwistStamped.h"
+#include <map>
+#include "arc_msgs/ToggleSchema.h"
 
 #ifndef ARC_BEHAVIOUR_ARCBASE_H
 #define ARC_BEHAVIOUR_ARCBASE_H
@@ -16,25 +16,28 @@ namespace arc_behaviour {
 
 class ArcBase {
 private:
-    /**
-     * MOTOR SCHEMAS
-     */
-
     /*
      * Node that this object is working on.
      */
     ros::NodeHandle *nh;
 
+    ros::NodeHandle global_handle;
+    ros::ServiceServer toggle_server;
+
     /**
      * Setup all of the motor schemas.
      */
-    bool setupSchemas(); //TODO: Throw exception if fail.
+    bool setupSchemas();
 
+    /**
+     * list of clients that will allow for toggling motor schemas.
+     */
+    std::map<std::string, ros::ServiceClient> motor_clients;
 public:
     ArcBase(ros::NodeHandle *nh);
 
     /**
-     * Activates schemas and ros publishers.
+     * Connect to various schemas.
      */
     void setup();
 
@@ -48,17 +51,21 @@ class ArcBase {
     /**
      * Turn on a specified motor schema.
      * @param type: Name of the schema to toggle, as specified in ros message request.
+     * @param state: True or false, depending on if you want to enable/disable schema.
      * @return bool: True if the schema was toggled, false if otherwise.
      */
-    bool toggleSchema(std::string type);
-
+    bool toggleSchema(std::string type, bool state);
 
     /**
      * Does calculation of final action vector by taking sum of all intermediate
      * motor schema action vectors.
      * @return Twist message representing final vector.
      */
     geometry_msgs::Twist getActionVector();
+
+    bool toggle_schema_cb(arc_msgs::ToggleSchema::Request &req, arc_msgs::ToggleSchema::Response &res);
+
+    void run();
 };
 };
 

arc_behaviour/include/DetectMarkerPS.h

@@ -9,62 +9,63 @@
 #ifndef ARC_BEHAVIOUR_MARKERDETECTORPS_H
 #define ARC_BEHAVIOUR_MARKERDETECTORPS_H
 
-namespace arc_behaviour {
+namespace arc_behaviour
+{
+    class DetectMarkerPS
+    {
+    private:
+        /**
+            * Collection of the markers found in given perceptual instance.
+            */
+        marker_msgs::MarkerDetection found_markers;
 
-class DetectMarkerPS {
-private:
-    /**
-     * Collection of the markers found in given perceptual instance.
-     */
-    marker_msgs::MarkerDetection found_markers;
+        /**
+            * Maximum range that we can detect markers from robots position.
+            */
+        int max_range;
 
-    /**
-     * Maximum range that we can detect markers from robots position.
-     */
-    int max_range;
+        /**
+            * Publish marker information, ie a simple boolean if markers are nearby.
+            */
+        ros::Publisher marker_status_publisher;
+        ros::Subscriber marker_detector_sub;
 
-    /**
-     * Publish marker information, ie a simple boolean if markers are nearby.
-     */
-    ros::Publisher marker_status_publisher;
-    ros::Subscriber marker_detector_sub;
+        //for public interface with ros
+        ros::NodeHandle *global_handle; //the handler for this node.
 
-    //for public interface with ros
-    ros::NodeHandle *global_handle; //the handler for this node.
+        //for private node material
+        ros::NodeHandle local_handle;
 
-    //for private node material
-    ros::NodeHandle local_handle;
+        //callbacks
 
-    //callbacks
+        //update us with most recent stage information
+        void process_detect_marker_cb(const marker_msgs::MarkerDetection &marker_info);
+    public:
+        /**
+            * Setup the detector with all of the ros topic publishers.
+            * @param handle : the node handle to setup with.
+            */
+        DetectMarkerPS();
 
-    //update us with most recent stage information
-    void process_detect_marker_cb(const marker_msgs::MarkerDetection marker_info);
-public:
-    /**
-     * Setup the detector with all of the ros topic publishers.
-     * @param handle : the node handle to setup with.
-     */
-    DetectMarkerPS();
+        /**
+            * Check output from stage and prune markers to ones only within our maximum range.
+            */
+        void ProcessStageFiducial();
 
-    /**
-     * Check output from stage and prune markers to ones only within our maximum range.
-     */
-    void ProcessStageFiducial();
+        /**
+            * @return True if at least 1 marker is nearby. False otherwise.
+            */
+        bool areMarkersNearby();
 
-    /**
-     * @return True if at least 1 marker is nearby. False otherwise.
-     */
-    bool areMarkersNearby();
+        void setMaxRange(int new_range);
 
-    void setMaxRange(int new_range);
+        ros::NodeHandle *getNodeHandle();
 
-    ros::NodeHandle *getNodeHandle();
-
-    /**
-     * Main loop. Publishes marker status information.
-     */
-    void run();
-};
+        /**
+            * Main loop. Publishes marker status information.
+            */
+        void run();
+    };
 }
 
 #endif //ARC_BEHAVIOUR_MARKERDETECTORPS_H

arc_behaviour/include/DetectRobotPS.h

@@ -71,6 +71,5 @@ class DetectRobotPS {
 };
 }
 
-
 #endif //ARC_BEHAVIOUR_DETECTROBOTPS_H
 //TODO: Change stage to not publish marker information, ie for every single marker on a topic. Just let robot detect it in map, and publish info when it finds them. We have to mod stage to move markers using a service, not by having cmd_vel publishing nonsense. ROS can't handle 100's of markers all publishing odom info, they aren't robots.

arc_behaviour/include/NavigationAdapter.h

@@ -59,7 +59,6 @@ namespace arc_behaviour {
          * @param req: The accepted navigation request.
          */
         void sendGoal(arc_msgs::NavigationRequest::Request &req);
-
     public:
         NavigationAdapter();
 
@@ -80,10 +79,21 @@ namespace arc_behaviour {
          */
         void move_to_goal_result_cb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
 
+        bool isGoal_active() const;
+
+        void setGoal_active(bool goal_active);
+
+        int getCurrent_nav_priority() const;
+
+        void setCurrent_nav_priority(int current_nav_priority);
+
+        bool isIs_stuck() const;
+
+        void setIs_stuck(bool is_stuck);
+
         //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/launch/default.launch

@@ -1,5 +1,7 @@
 <launch>
-    <node name="arc_base" pkg="arc_behaviour" type="arc_base" />
+    <node name="arc_base" pkg="arc_behaviour" type="arc_base">
+	<rosparam param="schemas">[random_wander_ms]</rosparam>
+    </node>
     <node name="detect_marker_ps" pkg="arc_behaviour" type="detect_marker_ps">
         <param name="max_range" value="5"/>
     </node>

arc_behaviour/launch/testing.launch

@@ -0,0 +1,26 @@
+<launch>
+    <node name="arc_base" pkg="arc_behaviour" type="arc_base">
+	<rosparam param="schemas">["random_wander_ms"]</rosparam>
+    </node>
+    <node name="detect_marker_ps" pkg="arc_behaviour" type="detect_marker_ps">
+        <param name="max_range" value="5"/>
+    </node>
+    <node name="detect_robot_ps" pkg="arc_behaviour" type="detect_robot_ps">
+        <param name="max_range" value="5"/>
+    </node>
+    <node name="detect_victim_ps" pkg="arc_behaviour" type="detect_victim_ps">
+        <param name="max_range" value="6"/>
+    </node>
+    <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>
+
+    <node name="random_wander_ms" pkg="arc_behaviour" type="random_wander_ms">
+	<param name="max_range" value="15"/>
+	<param name="update_goal_freq" value="0.05"/>
+	<param name="priority" value="1"/>
+    </node>
+</launch>

arc_behaviour/src/ArcBase.cpp

@@ -1,13 +1,45 @@
+#include <XmlRpcException.h>
 #include "../include/ArcBase.h"
+#include "ros/ros.h"
+#include "std_srvs/SetBool.h"
+#include "std_srvs/Empty.h"
 
 using namespace arc_behaviour;
+using namespace XmlRpc;
+#define DEFAULT_RATE 10
 
 ArcBase::ArcBase(ros::NodeHandle *nh) {
+    ros::NodeHandle global_handle;
     this->nh = nh;
+    this->global_handle = global_handle;
+    this->toggle_server = this->nh->advertiseService("toggle_schema", &ArcBase::toggle_schema_cb, this);
 }
 
 bool ArcBase::setupSchemas() {
     ROS_INFO("Setting up motor schemas.");
+    XmlRpcValue schema_list;
+
+    try {
+
+        ros::param::get("/arc_base/schemas", schema_list);
+
+        for(unsigned i=0; i< schema_list.size(); i++) {
+            ROS_DEBUG("Schema list contains: %s", schema_list[i]);
+            if(schema_list[i].getType()==XmlRpcValue::TypeString) {
+                std::string content = schema_list[i];
+                std::string topic_name = content + "/toggle";
+                ROS_INFO("Found schema: %s. subscribing to topic %s", content.c_str(), topic_name.c_str());
+                ros::ServiceClient client = this->global_handle.serviceClient<std_srvs::SetBool>(topic_name);
+
+                this->motor_clients.insert({content, client});
+            } else {
+                ROS_WARN("ArcBase::setupSchemas: found non-string value in schema list. value ignored.");
+            }
+        }
+    } catch(XmlRpc::XmlRpcException problem) {
+        ROS_WARN("error in arc_base::setupSchemas: %s. Make sure /arc_base/schemas param is set to strings.", problem.getMessage().c_str());
+    }
+
     return true;
 }
 
@@ -21,8 +53,28 @@ void ArcBase::setup() {
 }
 
 
-bool ArcBase::toggleSchema(std::string type) {
-    return false;
+bool ArcBase::toggleSchema(std::string type, bool state) {
+    std_srvs::SetBool req;
+    req.request.data = state;
+    this->motor_clients[type].call(req);
+}
+
+bool ArcBase::toggle_schema_cb(arc_msgs::ToggleSchema::Request &req, arc_msgs::ToggleSchema::Response &res) {
+    if(this->motor_clients[req.schema]!=NULL) {
+        this->toggleSchema(req.schema, req.state);
+    } else {
+        ROS_WARN("ArcBase::toggleSchema: %s was not found as a schema.", req.schema.c_str());
+    }
+    return true;
+}
+
+void ArcBase::run() {
+    ros::Rate r(DEFAULT_RATE);
+
+    while(ros::ok()) {
+        ros::spinOnce();
+        r.sleep();
+    }
 }
 
 geometry_msgs::Twist ArcBase::getActionVector() {

arc_behaviour/src/DetectDebrisPS.cpp

@@ -11,7 +11,6 @@
 #define PUBLISH_RATE 10
 
 //TODO: Test this overall schema. Setup a specific world and test to make sure it works properly on that world.
-//TODO: Find way to set /use_sim_time param to false by default, so these nodes publish. Right now you must manually disable using rosparam.
 using namespace arc_behaviour;
 
 DetectDebrisPS::DetectDebrisPS() {
@@ -26,7 +25,6 @@ DetectDebrisPS::DetectDebrisPS() {
 
     int max_range;
     local_handle.getParam("max_range", max_range);
-    //TODO: Stage has it's own max_range. Make stage use the max_range from parameter server, so they are both in sync. This way in stage the debris_detection will use the same max range as the behaviour module does.
     this->setMaxRange(max_range);
 }
 

arc_behaviour/src/DetectMarkerPS.cpp

@@ -9,14 +9,13 @@
 #define PUBLISH_RATE 10
 
 //TODO: Test this overall schema. Setup a specific world and test to make sure it works properly on that world.
-//TODO: Find way to set /use_sim_time param to false by default, so these nodes publish. Right now you must manually disable using rosparam.
 using namespace arc_behaviour;
 
 DetectMarkerPS::DetectMarkerPS() {
     ros::NodeHandle nh;
     this->global_handle = &nh;
     ros::NodeHandle local_handle("detect_marker_ps");
-    this->local_handle = local_handle;
+    this->local_handle = local_handle; //TODO: be consistent with pointers/nonpointer
     ROS_INFO("Setting up Marker detection perceptual schema.");
     this->marker_status_publisher = local_handle.advertise<std_msgs::Bool>("marker_status", MAX_QUEUE_SIZE);
     this->marker_detector_sub =  this->getNodeHandle()->subscribe("marker_detector", MAX_QUEUE_SIZE, &DetectMarkerPS::process_detect_marker_cb, this); //TODO: Check if publisher exists
@@ -28,7 +27,8 @@ DetectMarkerPS::DetectMarkerPS() {
     this->setMaxRange(max_range);
 }
 
-void DetectMarkerPS::process_detect_marker_cb(const marker_msgs::MarkerDetection marker_info) {
+void DetectMarkerPS::process_detect_marker_cb(const marker_msgs::MarkerDetection &marker_info)
+{
     ROS_DEBUG("process detect marker cb called.");
     this->found_markers = marker_info;
 }