State machine dev

mdis_state_machine/CMakeLists.txt

@@ -0,0 +1,95 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdis_state_machine)
+
+add_definitions(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  actionlib_msgs
+  actionlib
+  move_base_msgs
+  genmsg
+  sensor_msgs
+  geometry_msgs
+  nav_msgs
+  tf
+)
+
+add_message_files(
+  FILES
+  Connection.msg
+  DataCommunication.msg
+  RobotsState.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
+
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS 	roscpp
+                        rospy
+                        std_msgs
+                        actionlib_msgs
+                        actionlib
+                        move_base_msgs
+                        genmsg
+                        sensor_msgs
+                        geometry_msgs
+                        nav_msgs
+                        tf
+#  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}
+)
+
+###############
+## Libraries ##
+###############
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}
+src/move_base_interface.cpp
+src/robot_state_machine.cpp
+src/team_scheduler.cpp
+)
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+
+## Test Move base
+add_executable(move_base_test src/move_base_test.cpp)
+add_dependencies(move_base_test ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(move_base_test ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+## Test Move base
+add_executable(team_scheduler src/team_scheduler.cpp)
+add_dependencies(team_scheduler ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(team_scheduler ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+## Test Move base
+add_executable(connection_check src/connection_check.cpp)
+add_dependencies(connection_check ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(connection_check ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+install(TARGETS move_base_test
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+

mdis_state_machine/StateMachineReadMe.md

@@ -0,0 +1,66 @@
+# Instructions to test the scheduler/state machine
+
+## To Get started:
+
+### Background needed:
+* No additional node required
+* `roscore` NOT needed
+
+### Command to start
+```bash
+roslaunch mdis_state_machine test_state_machine.launch
+```
+
+## State machine summary:
+* Each robot runs its own state machine, only communicating with other robots when needed
+* Each state will continue unless its `transition` is triggered
+___
+## States flow:
+
+* Currently, there is no 'interrupt' for any of the states. They will simply be in their loops of the states
+* Each robot starts with a role, either explorer or a relay robot
+* Their inital tasks are defined as:
+  * RELAY: GO_TO_MEET
+  * EXPLORER: GO_TO_EXPLORE
+
+# States' loops: Explorer:
+
+> GO_TO_EXPLORE (1) ---> EXPLORE(2)
+* In actual scenarios, will be triggered after the robot reaches the exploration frontier
+* In testcases, will be triggered after 5 seconds
+
+> EXPLORE(2) ---> GO_TO_MEET(3)
+* In actual scenarios, will be triggered when the time alloted for exploration elapses
+* In test cases, will be triggered after 5 seconds (time calculations require robots' locations, map and move_base working)
+
+> GO_TO_MEET(3) ---> MEET(4)
+* In actual scenarios, will be triggered when the robot is connected to the party of interest
+* In test cases, the tester will have to emulate the connection by publishing the requied message over rostopic
+  * Currently, topic is `<ns>/connection_check`
+  * Message type is: `mdis_state_machine::Connection`
+* For the testing, use the connection between follows:
+  * `test_robot_1`or `test_robot_2` for depending on the robot type Explorer or Relay respectively
+  * Parent `dummy_parent`
+* When the line of sight is implemented, this can be changed to listen to whatever the new topic and new message type is
+
+> MEET(4) ---> GO_TO_EXPLORE (1)
+* In future, should be triggered when meeting communication is completed
+* Currently in code, it is an instantenious switch
+* In test cases, will be triggered after 5 seconds. 
+
+# States' loops: Relay:
+* In actual scenarios, Relay robot starts after 20 seconds (simple delay before start)
+* But in test cases, it will start right away
+* First state is GO_TO_MEET
+
+> GO_TO_MEET(3) ---> MEET(4)
+* Same as the transition `GO_TO_MEET(3) ---> MEET(4)` described above
+
+> MEET(4) ---> GO_TO_DUMP_DATA(5)
+* Same as the transition `MEET(4) ---> GO_TO_EXPLORE (1)` described above
+
+> GO_TO_DUMP_DATA(5) ---> DUMP_DATA(6)
+* Same as the transition `GO_TO_EXPLORE (1) ---> EXPLORE(2)` described above
+
+> DUMP_DATA(6) ---> GO_TO_MEET(3)
+* Same as the transition `MEET(4) ---> GO_TO_EXPLORE (1)` described above

mdis_state_machine/include/move_base_interface.h

@@ -0,0 +1,135 @@
+#include <actionlib/client/simple_action_client.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf/transform_listener.h>
+#include <move_base_msgs/MoveBaseAction.h>
+#include <actionlib_msgs/GoalStatus.h>
+#include <nav_msgs/GetPlan.h>
+#include <nav_msgs/Path.h>
+#include <tf/transform_listener.h>
+
+
+class MoveBaseInterface
+{
+  public:
+    MoveBaseInterface(ros::NodeHandle nh);
+    ~MoveBaseInterface(){};
+
+    /**
+     * @brief Get the Distance Prediction object
+     *        Requests plan from start to end and return 
+     *        the distance of the calculated plan
+     * 
+     * @param start_pose 
+     * @param end_pose 
+     * @return float distance of the calculated trajectory
+     */
+    float getDistancePrediction(geometry_msgs::PoseStamped &start_pose, geometry_msgs::PoseStamped &end_pose);
+
+    /**
+     * @brief Get the Distance Prediction object
+     *        Requests plan from start to end and return 
+     *        the distance of the calculated plan
+     * 
+     * @param start_pose 
+     * @param end_pose 
+     * @return float distance of the calculated trajectory
+     */
+    float getDistancePrediction(geometry_msgs::Point &start_point, geometry_msgs::Point &end_point);
+
+    /**
+     * @brief Get the Distance Prediction object
+     *        Requests plan from start to end and return 
+     *        the distance of the calculated plan
+     * 
+     * @param start_pose 
+     * @param end_pose 
+     * @return float distance of the calculated trajectory
+     */
+    float getDistancePrediction(geometry_msgs::Point &goal);
+
+    /**
+     * @brief Get the Time Prediction For Travel object
+     * 
+     * @param goal 
+     * @return float 
+     */
+    float getTimePredictionForTravel(geometry_msgs::Point &goal);
+
+    /**
+     * @brief Get the Time Prediction For Travel object
+     * 
+     * @param goal 
+     * @return float 
+     */
+    float getTimePredictionForTravel(geometry_msgs::Point &start_point, geometry_msgs::Point &end_point);
+
+    /**
+     * @brief Starts navigation and takes the robot to the location
+     * 
+     * @param goal_pose 
+     * @return true     Request accepted 
+     * @return false    Request rejected
+     */
+    bool goToPose(geometry_msgs::PoseStamped &goal_pose, bool wait_for_result = false);
+
+    /**
+     * @brief Starts navigation and takes the robot to the location
+     * 
+     * @param goal_pose 
+     * @return true     Request accepted 
+     * @return false    Request rejected
+     */
+    bool goToPoint(geometry_msgs::Point &goal, bool wait_for_result = false);
+
+    /**
+     * @brief Stops all the existing trajectories
+     * 
+     * @return true 
+     * @return false 
+     */
+    bool stopRobot();
+
+    /**
+     * @brief Get the Robot Current Pose object
+     * 
+     * @return geometry_msgs::PoseStamped    Robot's current pose
+     */
+    geometry_msgs::PoseStamped getRobotCurrentPose();
+
+
+    //////////// TO-DO ////////////
+    /**
+     * @brief In case the robot is stuck, this function should work as the recovery method
+     *        Drive back a meter, turn PI to the direction of the next path
+     *        Return after recovery complete
+     *        
+     * @return true 
+     * @return false 
+     */
+    bool recoverRobot();
+
+  private:
+    ros::NodeHandle nh_;
+    // typedef for move base
+    typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseTypedef;
+    MoveBaseTypedef *move_base_client_;
+
+    ros::ServiceClient move_base_planning_client_;
+    ros::Publisher debug_generated_path_pub;
+
+    tf::TransformListener tf_listener_;
+
+    move_base_msgs::MoveBaseGoal move_base_action_goal_;
+
+    std::string namespace_prefix = ros::this_node::getNamespace();
+    std::string logging_prefix_ = "[ "+ namespace_prefix +" | mdis_state_machine | move_base_interface ] ";
+    std::string map_frame;
+    std::string robot_frame;
+
+    const int MAX_ATTEMPTS = 5;
+    const float ROBOT_SPEED = (14/68.4); // Experimentally derived
+
+    float calculatePathLength(const nav_msgs::Path& path);
+
+    geometry_msgs::PoseStamped transformTf2msg(const tf::StampedTransform& transform);
+};