From 8c6b310de4968681ddc3f86a0c31b0e79054e25b Mon Sep 17 00:00:00 2001 From: Ashay Aswale Date: Sat, 4 Dec 2021 02:57:13 -0500 Subject: [PATCH 01/12] move base interface for the state machine --- mdis_state_machine/CMakeLists.txt | 68 ++++++++++++ .../include/move_base_interface.h | 89 +++++++++++++++ mdis_state_machine/package.xml | 53 +++++++++ .../src/move_base_interface.cpp | 102 ++++++++++++++++++ mdis_state_machine/src/move_base_test.cpp | 18 ++++ 5 files changed, 330 insertions(+) create mode 100644 mdis_state_machine/CMakeLists.txt create mode 100644 mdis_state_machine/include/move_base_interface.h create mode 100644 mdis_state_machine/package.xml create mode 100644 mdis_state_machine/src/move_base_interface.cpp create mode 100644 mdis_state_machine/src/move_base_test.cpp diff --git a/mdis_state_machine/CMakeLists.txt b/mdis_state_machine/CMakeLists.txt new file mode 100644 index 0000000..9367658 --- /dev/null +++ b/mdis_state_machine/CMakeLists.txt @@ -0,0 +1,68 @@ +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 +) + + +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 +# 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 +) +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}) + +install(TARGETS move_base_test + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + diff --git a/mdis_state_machine/include/move_base_interface.h b/mdis_state_machine/include/move_base_interface.h new file mode 100644 index 0000000..024b077 --- /dev/null +++ b/mdis_state_machine/include/move_base_interface.h @@ -0,0 +1,89 @@ +#include +#include +#include +#include +#include +#include + + +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 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(); + + //////////// 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 MoveBaseTypedef; + MoveBaseTypedef *move_base_client_; + + ros::ServiceClient move_base_planning_client_; + ros::Publisher debug_generated_path_pub; + + 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 ] "; + + float calculatePathLength(const nav_msgs::Path& path); +}; \ No newline at end of file diff --git a/mdis_state_machine/package.xml b/mdis_state_machine/package.xml new file mode 100644 index 0000000..ed35e47 --- /dev/null +++ b/mdis_state_machine/package.xml @@ -0,0 +1,53 @@ + + + mdis_state_machine + 0.1.0 + The mdis_state_machine package + + Ashay + + TODO + + + + catkin + + roscpp + rospy + std_msgs + actionlib + actionlib_msgs + move_base_msgs + genmsg + sensor_msgs + geometry_msgs + nav_msgs + + roscpp + rospy + std_msgs + actionlib + actionlib_msgs + move_base_msgs + genmsg + sensor_msgs + geometry_msgs + nav_msgs + + roscpp + rospy + std_msgs + actionlib + actionlib_msgs + move_base_msgs + genmsg + sensor_msgs + geometry_msgs + nav_msgs + + + + + + + diff --git a/mdis_state_machine/src/move_base_interface.cpp b/mdis_state_machine/src/move_base_interface.cpp new file mode 100644 index 0000000..6c2410c --- /dev/null +++ b/mdis_state_machine/src/move_base_interface.cpp @@ -0,0 +1,102 @@ +#include + +MoveBaseInterface::MoveBaseInterface(ros::NodeHandle nh): nh_(nh) +{ + move_base_client_ = new MoveBaseTypedef("move_base", true); + move_base_planning_client_ = nh.serviceClient("move_base/make_plan"); + debug_generated_path_pub = nh.advertise("debug_plan_path", 1000); + + ROS_INFO_STREAM(logging_prefix_ << "Waiting for actionlib servers"); + move_base_client_->waitForServer(); + ROS_INFO_STREAM(logging_prefix_ << "All servers started"); + + namespace_prefix.erase(namespace_prefix.begin()); +} + +bool MoveBaseInterface::goToPose(geometry_msgs::PoseStamped &goal_pose, bool wait_for_result) +{ + move_base_action_goal_.target_pose = goal_pose; + move_base_client_->sendGoal(move_base_action_goal_); + ROS_INFO_STREAM(logging_prefix_ << "Waiting"); + if(wait_for_result) + { + move_base_client_->waitForResult(); + + // Doesn't work, + // @to-do fix later + // return move_base_client_->getState().status_list.front() == actionlib_msgs::GoalStatus::SUCCEEDED; + } + return true; +} + +bool MoveBaseInterface::goToPoint(geometry_msgs::Point &goal, bool wait_for_result) +{ + move_base_action_goal_.target_pose.pose.position = goal; + move_base_action_goal_.target_pose.pose.orientation.w = 1.; + move_base_action_goal_.target_pose.header.frame_id = namespace_prefix+"/map"; + move_base_action_goal_.target_pose.header.stamp = ros::Time::now(); + move_base_client_->sendGoal(move_base_action_goal_); + ROS_INFO_STREAM(logging_prefix_ << "Waiting"); + if(wait_for_result) + { + move_base_client_->waitForResult(); + + // Doesn't work, + // @to-do fix later + // return move_base_client_->getState().status_list.front() == actionlib_msgs::GoalStatus::SUCCEEDED; + } + return true; +} + +bool MoveBaseInterface::stopRobot() +{ + move_base_client_->cancelGoal(); + return true; +} + +float MoveBaseInterface::getDistancePrediction(geometry_msgs::PoseStamped &start_pose, geometry_msgs::PoseStamped &end_pose) +{ + nav_msgs::GetPlan srv; + srv.request.start = start_pose; + srv.request.goal = end_pose; + move_base_planning_client_.call(srv); + nav_msgs::Path path = srv.response.plan; + return calculatePathLength(path); +} + +float MoveBaseInterface::getDistancePrediction(geometry_msgs::Point &start_point, geometry_msgs::Point &end_point) +{ + nav_msgs::GetPlan srv; + srv.request.start.pose.position = start_point; + srv.request.start.pose.orientation.w = 1.; + srv.request.start.header.frame_id = namespace_prefix+"/map"; + srv.request.start.header.stamp = ros::Time::now(); + + srv.request.goal.pose.position = end_point; + srv.request.goal.pose.orientation.w = 1.; + srv.request.goal.header.frame_id = namespace_prefix+"/map"; + srv.request.goal.header.stamp = ros::Time::now(); + + move_base_planning_client_.call(srv); + nav_msgs::Path path = srv.response.plan; + path.header.frame_id = namespace_prefix+"/map"; + path.header.stamp = ros::Time::now(); + debug_generated_path_pub.publish(path); + return calculatePathLength(path); +} + +float MoveBaseInterface::calculatePathLength(const nav_msgs::Path& path) +{ + double length = 0; + float last_x, last_y; + for(int i = 0; i + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "move_base_test"); + ros::NodeHandle nh; + MoveBaseInterface explore(nh); + // ros::Duration(2).sleep(); + geometry_msgs::Point point_1, point_2; + point_1.x = -31; + point_1.y = -8; + point_2.x = -31; + point_2.y = -6; + float dist = explore.getDistancePrediction(point_1, point_2); + ROS_INFO_STREAM("Distance: "< Date: Sat, 4 Dec 2021 23:53:56 -0500 Subject: [PATCH 02/12] Scheduler and state machine skeleton --- mdis_state_machine/CMakeLists.txt | 11 +- .../include/move_base_interface.h | 33 +++- .../include/robot_state_machine.h | 109 ++++++++++++ mdis_state_machine/include/team_scheduler.h | 28 ++++ mdis_state_machine/package.xml | 3 + .../src/move_base_interface.cpp | 73 +++++++- mdis_state_machine/src/move_base_test.cpp | 6 +- .../src/robot_state_machine.cpp | 156 ++++++++++++++++++ mdis_state_machine/src/team_scheduler.cpp | 136 +++++++++++++++ 9 files changed, 548 insertions(+), 7 deletions(-) create mode 100644 mdis_state_machine/include/robot_state_machine.h create mode 100644 mdis_state_machine/include/team_scheduler.h create mode 100644 mdis_state_machine/src/robot_state_machine.cpp create mode 100644 mdis_state_machine/src/team_scheduler.cpp diff --git a/mdis_state_machine/CMakeLists.txt b/mdis_state_machine/CMakeLists.txt index 9367658..03f9d92 100644 --- a/mdis_state_machine/CMakeLists.txt +++ b/mdis_state_machine/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs geometry_msgs nav_msgs + tf ) @@ -30,6 +31,7 @@ catkin_package( sensor_msgs geometry_msgs nav_msgs + tf # DEPENDS system_lib ) @@ -51,16 +53,23 @@ include_directories( ## 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_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}) + install(TARGETS move_base_test ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/mdis_state_machine/include/move_base_interface.h b/mdis_state_machine/include/move_base_interface.h index 024b077..ae4aab3 100644 --- a/mdis_state_machine/include/move_base_interface.h +++ b/mdis_state_machine/include/move_base_interface.h @@ -1,9 +1,11 @@ #include #include +#include #include #include #include #include +#include class MoveBaseInterface @@ -34,6 +36,17 @@ class MoveBaseInterface */ 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 Starts navigation and takes the robot to the location * @@ -60,6 +73,14 @@ class MoveBaseInterface */ 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 @@ -79,11 +100,19 @@ class MoveBaseInterface 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 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; float calculatePathLength(const nav_msgs::Path& path); + + geometry_msgs::PoseStamped transformTf2msg(const tf::StampedTransform& transform); }; \ No newline at end of file diff --git a/mdis_state_machine/include/robot_state_machine.h b/mdis_state_machine/include/robot_state_machine.h new file mode 100644 index 0000000..f7fa0f4 --- /dev/null +++ b/mdis_state_machine/include/robot_state_machine.h @@ -0,0 +1,109 @@ +#pragma once + +#include + +enum TEAM_STATES{ + IDLE, + GO_TO_EXPLORE, + EXPLORE, + GO_TO_MEET, + MEET, +}; + +class RobotState { + +public: + + RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHandle &nh); + + ~RobotState() + { + } + + uint64_t getId() const { return m_unId; } + + const std::string& getName() const { return m_strName; } + + virtual bool entryPoint() = 0; + + virtual void exitPoint() = 0; + + virtual void step() = 0; + + virtual bool isDone() = 0; + + virtual TEAM_STATES transition() = 0; + +protected: + + uint64_t m_unId; + std::string m_strName; + + bool meeting_started, go_for_exploration; +}; + +class Idle: public RobotState{ +public: + Idle(ros::NodeHandle &nh):RobotState(IDLE, "Idle", nh){} + bool isDone() override ; + + TEAM_STATES transition() override; + bool entryPoint() override; + void step() override; + void exitPoint() override; +}; + + +class GoToExplore: public RobotState{ +public: + GoToExplore(ros::NodeHandle &nh):RobotState(GO_TO_EXPLORE, "GoToExplore", nh){} + bool isDone() override ; + + TEAM_STATES transition() override; + + bool entryPoint() override; + void step() override; + void exitPoint() override; +}; + + +class Explore: public RobotState{ +public: + Explore(ros::NodeHandle &nh):RobotState(EXPLORE, "Explore", nh){} + bool isDone() override ; + + TEAM_STATES transition() override; + + bool entryPoint() override; + void step() override; + void exitPoint() override; +}; + + +class GoToMeet: public RobotState{ +public: + GoToMeet(ros::NodeHandle &nh):RobotState(GO_TO_MEET, "GoToMeet", nh){} + bool isDone() override ; + + TEAM_STATES transition() override; + + bool entryPoint() override; + void step() override; + void exitPoint() override; +}; + + +class Meet: public RobotState{ +public: + Meet(ros::NodeHandle &nh):RobotState(MEET, "Meet", nh){} + bool isDone() override ; + + TEAM_STATES transition() override; + + bool entryPoint() override; + void step() override; + void exitPoint() override; +}; + + + diff --git a/mdis_state_machine/include/team_scheduler.h b/mdis_state_machine/include/team_scheduler.h new file mode 100644 index 0000000..3de5eff --- /dev/null +++ b/mdis_state_machine/include/team_scheduler.h @@ -0,0 +1,28 @@ +#pragma once + +#include +#include + +class TeamScheduler{ +public: + TeamScheduler(ros::NodeHandle &nh); + ~TeamScheduler(); + + void exec(); + void step(); + + RobotState& getStatePtr(uint64_t un_id); + void setTeamMacroState(TEAM_STATES state){ + robot_state = state;} + +private: + void addStates(ros::NodeHandle &nh); + void addState(RobotState* RobotStatePtr); + TEAM_STATES robot_state; + bool new_state_request; + + void setInitialState(uint64_t un_state); + + RobotState* current_state_ptr; + std::unordered_map MACRO_STATE_PTR_MAP; +}; diff --git a/mdis_state_machine/package.xml b/mdis_state_machine/package.xml index ed35e47..55a4a0f 100644 --- a/mdis_state_machine/package.xml +++ b/mdis_state_machine/package.xml @@ -22,6 +22,7 @@ sensor_msgs geometry_msgs nav_msgs + tf roscpp rospy @@ -33,6 +34,7 @@ sensor_msgs geometry_msgs nav_msgs + tf roscpp rospy @@ -44,6 +46,7 @@ sensor_msgs geometry_msgs nav_msgs + tf diff --git a/mdis_state_machine/src/move_base_interface.cpp b/mdis_state_machine/src/move_base_interface.cpp index 6c2410c..5c64907 100644 --- a/mdis_state_machine/src/move_base_interface.cpp +++ b/mdis_state_machine/src/move_base_interface.cpp @@ -11,6 +11,8 @@ MoveBaseInterface::MoveBaseInterface(ros::NodeHandle nh): nh_(nh) ROS_INFO_STREAM(logging_prefix_ << "All servers started"); namespace_prefix.erase(namespace_prefix.begin()); + map_frame = namespace_prefix+"/map"; + robot_frame = namespace_prefix+"/base_footprint"; } bool MoveBaseInterface::goToPose(geometry_msgs::PoseStamped &goal_pose, bool wait_for_result) @@ -69,17 +71,35 @@ float MoveBaseInterface::getDistancePrediction(geometry_msgs::Point &start_point nav_msgs::GetPlan srv; srv.request.start.pose.position = start_point; srv.request.start.pose.orientation.w = 1.; - srv.request.start.header.frame_id = namespace_prefix+"/map"; + srv.request.start.header.frame_id = map_frame; srv.request.start.header.stamp = ros::Time::now(); srv.request.goal.pose.position = end_point; srv.request.goal.pose.orientation.w = 1.; - srv.request.goal.header.frame_id = namespace_prefix+"/map"; + srv.request.goal.header.frame_id = map_frame; srv.request.goal.header.stamp = ros::Time::now(); move_base_planning_client_.call(srv); nav_msgs::Path path = srv.response.plan; - path.header.frame_id = namespace_prefix+"/map"; + path.header.frame_id = map_frame; + path.header.stamp = ros::Time::now(); + debug_generated_path_pub.publish(path); + return calculatePathLength(path); +} + +float MoveBaseInterface::getDistancePrediction(geometry_msgs::Point &goal) +{ + nav_msgs::GetPlan srv; + srv.request.start = getRobotCurrentPose(); + + srv.request.goal.pose.position = goal; + srv.request.goal.pose.orientation.w = 1.; + srv.request.goal.header.frame_id = map_frame; + srv.request.goal.header.stamp = ros::Time::now(); + + move_base_planning_client_.call(srv); + nav_msgs::Path path = srv.response.plan; + path.header.frame_id = map_frame; path.header.stamp = ros::Time::now(); debug_generated_path_pub.publish(path); return calculatePathLength(path); @@ -99,4 +119,51 @@ float MoveBaseInterface::calculatePathLength(const nav_msgs::Path& path) last_y = y; } return length; +} + +geometry_msgs::PoseStamped MoveBaseInterface::getRobotCurrentPose() +{ + int attempt = 0; + bool not_found_tf = true; + tf::StampedTransform transform; + geometry_msgs::PoseStamped result_transform; + + result_transform.header.frame_id = map_frame; + result_transform.header.stamp = ros::Time::now(); + + while (attempt++ < MAX_ATTEMPTS && not_found_tf && ros::ok()){ + try{ + tf_listener_.lookupTransform(map_frame, robot_frame, + ros::Time(0), transform); + + not_found_tf = false; + } + catch (tf::TransformException ex){ + ROS_WARN("%s",ex.what()); + ros::Duration(1.0).sleep(); + } + } + + if(not_found_tf) + ROS_ERROR("Could not find the transform after %i attempts", MAX_ATTEMPTS); + else + result_transform = transformTf2msg(transform); + + return result_transform; +} + +geometry_msgs::PoseStamped MoveBaseInterface::transformTf2msg(const tf::StampedTransform& transform) +{ + geometry_msgs::PoseStamped msg; + + msg.pose.position.x = transform.getOrigin().x(); + msg.pose.position.y = transform.getOrigin().y(); + msg.pose.position.z = transform.getOrigin().z(); + + msg.pose.orientation.x = transform.getRotation().x(); + msg.pose.orientation.y = transform.getRotation().y(); + msg.pose.orientation.z = transform.getRotation().z(); + msg.pose.orientation.w = transform.getRotation().w(); + + return msg; } \ No newline at end of file diff --git a/mdis_state_machine/src/move_base_test.cpp b/mdis_state_machine/src/move_base_test.cpp index 3f55b77..a318958 100644 --- a/mdis_state_machine/src/move_base_test.cpp +++ b/mdis_state_machine/src/move_base_test.cpp @@ -11,8 +11,12 @@ int main(int argc, char** argv) point_1.y = -8; point_2.x = -31; point_2.y = -6; - float dist = explore.getDistancePrediction(point_1, point_2); + float dist = explore.getDistancePrediction(point_2); ROS_INFO_STREAM("Distance: "< + +RobotState::RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHandle &nh) : + // m_pcTeam(nullptr), + m_unId(un_id), m_strName(str_name) +{ +} + + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////// I D L E S T A T E C L A S S //////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +bool Idle::entryPoint() +{ + return true; +} + +bool Idle::isDone() +{ + return true; +} + + +TEAM_STATES Idle::transition() +{ + return GO_TO_EXPLORE; +} + +void Idle::step() +{ + ROS_INFO("Step for Idle"); +} + +void Idle::exitPoint() +{ +} + + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////// G O T O E X P L O R E S T A T E C L A S S //////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +bool GoToExplore::entryPoint() +{ + return true; +} + +bool GoToExplore::isDone() +{ + return true; +} + + +TEAM_STATES GoToExplore::transition() +{ + return EXPLORE; +} + +void GoToExplore::step() +{ + ROS_INFO("Step for GoToExplore"); +} + +void GoToExplore::exitPoint() +{ +} + + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////// E X P L O R E S T A T E C L A S S //////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +bool Explore::entryPoint() +{ + return true; +} + +bool Explore::isDone() +{ + return true; +} + + +TEAM_STATES Explore::transition() +{ + return GO_TO_MEET; +} + +void Explore::step() +{ + ROS_INFO("Step for Explore"); +} + +void Explore::exitPoint() +{ +} + + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////// G O T O M E E T S T A T E C L A S S //////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +bool GoToMeet::entryPoint() +{ + return true; +} + +bool GoToMeet::isDone() +{ + return true; +} + + +TEAM_STATES GoToMeet::transition() +{ + return MEET; +} + +void GoToMeet::step() +{ + ROS_INFO("Step for GoToMeet"); +} + +void GoToMeet::exitPoint() +{ +} + + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////// M E E T S T A T E C L A S S //////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +bool Meet::entryPoint() +{ + return true; +} + +bool Meet::isDone() +{ + return true; +} + +TEAM_STATES Meet::transition() +{ + return IDLE; +} + +void Meet::step() +{ + ROS_INFO("Step for Meet"); +} + +void Meet::exitPoint() +{ +} diff --git a/mdis_state_machine/src/team_scheduler.cpp b/mdis_state_machine/src/team_scheduler.cpp new file mode 100644 index 0000000..332999a --- /dev/null +++ b/mdis_state_machine/src/team_scheduler.cpp @@ -0,0 +1,136 @@ +#include + +TeamScheduler::TeamScheduler(ros::NodeHandle &nh) +{ + addStates(nh); + setInitialState(IDLE); +} + +TeamScheduler::~TeamScheduler() +{ + std::for_each( + MACRO_STATE_PTR_MAP.begin(), + MACRO_STATE_PTR_MAP.end(), + [](std::pair c_item){ + delete c_item.second; + }); +} + +void TeamScheduler::addStates(ros::NodeHandle &nh) +{ + addState(new Idle(nh)); + addState(new GoToExplore(nh)); + addState(new Explore(nh)); + addState(new GoToMeet(nh)); + addState(new Meet(nh)); +} + +void TeamScheduler::addState(RobotState* pc_state) { + if(MACRO_STATE_PTR_MAP.find(pc_state->getId()) == MACRO_STATE_PTR_MAP.end()) { + MACRO_STATE_PTR_MAP[pc_state->getId()] = pc_state; + // pc_state->setTeam(*this); + } + else { + ROS_ERROR_STREAM("[mdis_state_machine | team_scheduler.cpp ]: Duplicated state id :" << pc_state->getId()); + } +} + +// TEAM_STATES RobotState::getState(uint64_t un_state) { +// return m_pcTeam->getState(un_state); +// } + +RobotState& TeamScheduler::getStatePtr(uint64_t un_id) +{ + auto pcState = MACRO_STATE_PTR_MAP.find(un_id); + if(pcState != MACRO_STATE_PTR_MAP.end()) { + return *(pcState->second); + } + else { + ROS_ERROR_STREAM("[TEAM_LEVEL | team_scheduler ]:Can't get state id " << un_id); + } +} + +void TeamScheduler::setInitialState(uint64_t un_state) { + auto pcState = MACRO_STATE_PTR_MAP.find(un_state); + // if state exists in map, then set it to the initial state of the scheduler + if(pcState != MACRO_STATE_PTR_MAP.end()) { + // acquire value of the state (every map has a key(first) and a value(second)) + current_state_ptr = pcState->second; + + // completes entry point of the initial state + current_state_ptr->entryPoint(); + setTeamMacroState((TEAM_STATES) un_state); + } + else { + ROS_ERROR_STREAM("[TEAM_LEVEL | team_scheduler ]: Can't set initial state to " << un_state); + } +} + +void TeamScheduler::step() { + /* Only execute if 'current' was initialized */ + if(current_state_ptr) { + // ROS_INFO_STREAM(hired_scout << " " << hired_excavator << " " << hired_hauler); + // ROS_INFO_STREAM("Robot enum:" << SCOUT_1); + /* Attempt a transition, every state of every rover has its own transition() */ + TEAM_STATES newStateEnum = current_state_ptr->transition(); + RobotState* cNewState = &getStatePtr(newStateEnum); + // RobotState* cNewState = current_state_ptr; + if (new_state_request) + { + cNewState = &getStatePtr(robot_state); + new_state_request = false; + } + + if(cNewState != current_state_ptr) { + /* Perform transition */ + current_state_ptr->exitPoint(); + // cNewState->setResetRobot(reset_robot_odometry); + bool entry = cNewState->entryPoint(); + + setTeamMacroState((TEAM_STATES) cNewState->getId()); + if(!entry) + return; + current_state_ptr = cNewState; + } + /* Execute current state */ + current_state_ptr->step(); + } + else { + ROS_ERROR_STREAM("[TEAM_LEVEL | team_scheduler ]: The Team has not been initialized, you must call SetInitialState()"); + } +} + +// /****************************************/ +// /****************************************/ + +//UNDERSTANDING: Each robot has its own done() and this is what is checked to perform step() +void TeamScheduler::exec() { + ros::Rate r(50); // 50Hz loop rate + while(ros::ok()) + { + step(); + ros::spinOnce(); + r.sleep(); + } +} + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "state_machine"); + ros::NodeHandle nh; + + ROS_INFO("One"); + TeamScheduler team(nh); + ROS_INFO("exec"); + team.exec(); + + // ros::Duration(1).sleep(); +// team.setTeamMacroState(SEARCH); +// ros::Duration(1).sleep(); +// team.setTeamMacroState(SEARCH); +// ros::Duration(1).sleep(); +// team.setTeamMacroState(SEARCH); +// ros::spinOnce(); + return 0; +} From 4b8566acda4707981664fefe23b2371f169470ee Mon Sep 17 00:00:00 2001 From: Ashay Aswale Date: Sun, 5 Dec 2021 03:39:32 -0500 Subject: [PATCH 03/12] tendons and tissues to skeleton --- .../include/robot_state_machine.h | 21 +++++- .../src/robot_state_machine.cpp | 64 ++++++++++++++++--- mdis_state_machine/src/team_scheduler.cpp | 9 +-- 3 files changed, 77 insertions(+), 17 deletions(-) diff --git a/mdis_state_machine/include/robot_state_machine.h b/mdis_state_machine/include/robot_state_machine.h index f7fa0f4..69e0861 100644 --- a/mdis_state_machine/include/robot_state_machine.h +++ b/mdis_state_machine/include/robot_state_machine.h @@ -1,6 +1,7 @@ #pragma once #include +#include enum TEAM_STATES{ IDLE, @@ -8,6 +9,8 @@ enum TEAM_STATES{ EXPLORE, GO_TO_MEET, MEET, + GO_TO_DUMP_DATA, + DUMP_DATA, }; class RobotState { @@ -34,12 +37,19 @@ class RobotState { virtual TEAM_STATES transition() = 0; + geometry_msgs::Point current_meeting_location, next_meeting_location; + protected: uint64_t m_unId; std::string m_strName; + bool is_explorer; bool meeting_started, go_for_exploration; + + MoveBaseInterface *explore_interface; + + ros::Duration time_until_next_meeting = ros::Duration(30.0); }; class Idle: public RobotState{ @@ -51,6 +61,9 @@ class Idle: public RobotState{ bool entryPoint() override; void step() override; void exitPoint() override; + +private: + ros::Publisher chatter_pub; }; @@ -69,7 +82,9 @@ class GoToExplore: public RobotState{ class Explore: public RobotState{ public: - Explore(ros::NodeHandle &nh):RobotState(EXPLORE, "Explore", nh){} + Explore(ros::NodeHandle &nh):RobotState(EXPLORE, "Explore", nh){ + pause_exploration_pub = nh.advertise("explore/pause_exploration", 1000); + } bool isDone() override ; TEAM_STATES transition() override; @@ -77,6 +92,10 @@ class Explore: public RobotState{ bool entryPoint() override; void step() override; void exitPoint() override; + +private: + ros::Publisher pause_exploration_pub; + ros::Time starting_time; }; diff --git a/mdis_state_machine/src/robot_state_machine.cpp b/mdis_state_machine/src/robot_state_machine.cpp index 9fbe11e..a961cf8 100644 --- a/mdis_state_machine/src/robot_state_machine.cpp +++ b/mdis_state_machine/src/robot_state_machine.cpp @@ -4,6 +4,12 @@ RobotState::RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHan // m_pcTeam(nullptr), m_unId(un_id), m_strName(str_name) { + explore_interface = new MoveBaseInterface(nh); + + current_meeting_location.x = -31; + current_meeting_location.y = -6; + next_meeting_location.x = -28; + next_meeting_location.y = -6; } @@ -13,6 +19,7 @@ RobotState::RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHan bool Idle::entryPoint() { + explore_interface->stopRobot(); return true; } @@ -48,13 +55,17 @@ bool GoToExplore::entryPoint() bool GoToExplore::isDone() { + ROS_INFO_STREAM("Going to explore at"<goToPoint(next_meeting_location, true); return true; } - TEAM_STATES GoToExplore::transition() { - return EXPLORE; + if(isDone()) + return EXPLORE; + else + return GO_TO_EXPLORE; } void GoToExplore::step() @@ -73,27 +84,47 @@ void GoToExplore::exitPoint() bool Explore::entryPoint() { + starting_time = ros::Time::now(); return true; } bool Explore::isDone() { - return true; + ros::Duration time_since_start = ros::Time::now() - starting_time; + return time_since_start > time_until_next_meeting; } - TEAM_STATES Explore::transition() { - return GO_TO_MEET; + if(isDone()) + return GO_TO_MEET; + else + return EXPLORE; } void Explore::step() { - ROS_INFO("Step for Explore"); + std_msgs::Bool msg; + msg.data = false; + pause_exploration_pub.publish(msg); + ROS_INFO_THROTTLE(10,"Step for Explore"); } void Explore::exitPoint() { + std_msgs::Bool msg; + msg.data = true; + pause_exploration_pub.publish(msg); + + // TO-DO + // BUG!!!! + // Values are being changed only for the class Explore + // Need to change values for all + next_meeting_location = explore_interface->getRobotCurrentPose().pose.position; + + ROS_INFO_STREAM("Next meeting"<stopRobot(); } @@ -103,18 +134,26 @@ void Explore::exitPoint() bool GoToMeet::entryPoint() { + ROS_INFO_STREAM("Going to meet at"<goToPoint(current_meeting_location, false); return true; } bool GoToMeet::isDone() { + // Connection established + + // Temporary case + ros::Duration(20).sleep(); return true; } TEAM_STATES GoToMeet::transition() { - return MEET; + if(isDone()) + return MEET; + else GO_TO_MEET; } void GoToMeet::step() @@ -124,6 +163,7 @@ void GoToMeet::step() void GoToMeet::exitPoint() { + explore_interface->stopRobot(); } @@ -138,12 +178,16 @@ bool Meet::entryPoint() bool Meet::isDone() { + // Data sync done return true; } TEAM_STATES Meet::transition() { - return IDLE; + if(isDone()) + return GO_TO_EXPLORE; + else + return MEET; } void Meet::step() @@ -153,4 +197,8 @@ void Meet::step() void Meet::exitPoint() { + current_meeting_location = next_meeting_location; + + // get time estimate from move_base_interface + // set exploration_duration accordingly } diff --git a/mdis_state_machine/src/team_scheduler.cpp b/mdis_state_machine/src/team_scheduler.cpp index 332999a..2470c7e 100644 --- a/mdis_state_machine/src/team_scheduler.cpp +++ b/mdis_state_machine/src/team_scheduler.cpp @@ -105,7 +105,7 @@ void TeamScheduler::step() { //UNDERSTANDING: Each robot has its own done() and this is what is checked to perform step() void TeamScheduler::exec() { - ros::Rate r(50); // 50Hz loop rate + ros::Rate r(5); // 50Hz loop rate while(ros::ok()) { step(); @@ -125,12 +125,5 @@ int main(int argc, char** argv) ROS_INFO("exec"); team.exec(); - // ros::Duration(1).sleep(); -// team.setTeamMacroState(SEARCH); -// ros::Duration(1).sleep(); -// team.setTeamMacroState(SEARCH); -// ros::Duration(1).sleep(); -// team.setTeamMacroState(SEARCH); -// ros::spinOnce(); return 0; } From 04bcd4bd513313d08136eac09cb29d0f319e20a0 Mon Sep 17 00:00:00 2001 From: Ashay Aswale Date: Mon, 6 Dec 2021 13:12:19 -0500 Subject: [PATCH 04/12] dirty fix to static issue --- .../include/robot_state_machine.h | 33 ++++++++++++++++++- .../src/robot_state_machine.cpp | 30 ++++++++++------- 2 files changed, 51 insertions(+), 12 deletions(-) diff --git a/mdis_state_machine/include/robot_state_machine.h b/mdis_state_machine/include/robot_state_machine.h index 69e0861..f72d516 100644 --- a/mdis_state_machine/include/robot_state_machine.h +++ b/mdis_state_machine/include/robot_state_machine.h @@ -37,8 +37,8 @@ class RobotState { virtual TEAM_STATES transition() = 0; - geometry_msgs::Point current_meeting_location, next_meeting_location; + static float curr_meet_x, curr_meet_y, next_meet_x, next_meet_y; protected: uint64_t m_unId; @@ -50,6 +50,37 @@ class RobotState { MoveBaseInterface *explore_interface; ros::Duration time_until_next_meeting = ros::Duration(30.0); + + + void setCurrentMeetingLocation(const geometry_msgs::Point& meeting) + { + curr_meet_x = meeting.x; + curr_meet_y = meeting.y; + } + void setNextMeetingLocation(const geometry_msgs::Point& meeting) + { + next_meet_x = meeting.x; + next_meet_y = meeting.y; + } + geometry_msgs::Point getCurrentMeetingPoint() + { + geometry_msgs::Point point; + point.x = curr_meet_x; + point.y = curr_meet_y; + return point; + } + geometry_msgs::Point getNextMeetingPoint() + { + geometry_msgs::Point point; + point.x = next_meet_x; + point.y = next_meet_y; + return point; + } + void setCurrAsNextMeeting() + { + curr_meet_x = next_meet_x; + curr_meet_y = next_meet_y; + } }; class Idle: public RobotState{ diff --git a/mdis_state_machine/src/robot_state_machine.cpp b/mdis_state_machine/src/robot_state_machine.cpp index a961cf8..1f7eb6c 100644 --- a/mdis_state_machine/src/robot_state_machine.cpp +++ b/mdis_state_machine/src/robot_state_machine.cpp @@ -1,15 +1,21 @@ #include +float RobotState::curr_meet_x = 0.0; +float RobotState::curr_meet_y = 0.0; +float RobotState::next_meet_x = 0.0; +float RobotState::next_meet_y = 0.0; + RobotState::RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHandle &nh) : // m_pcTeam(nullptr), m_unId(un_id), m_strName(str_name) { explore_interface = new MoveBaseInterface(nh); - current_meeting_location.x = -31; - current_meeting_location.y = -6; - next_meeting_location.x = -28; - next_meeting_location.y = -6; + curr_meet_x = -31; + curr_meet_y = -6; + next_meet_x = -28; + next_meet_y = -6; + } @@ -55,8 +61,9 @@ bool GoToExplore::entryPoint() bool GoToExplore::isDone() { - ROS_INFO_STREAM("Going to explore at"<goToPoint(next_meeting_location, true); + ROS_INFO_STREAM("Going to explore at"<goToPoint(temp_point, true); return true; } @@ -120,9 +127,9 @@ void Explore::exitPoint() // BUG!!!! // Values are being changed only for the class Explore // Need to change values for all - next_meeting_location = explore_interface->getRobotCurrentPose().pose.position; + setNextMeetingLocation(explore_interface->getRobotCurrentPose().pose.position); - ROS_INFO_STREAM("Next meeting"<stopRobot(); } @@ -134,8 +141,9 @@ void Explore::exitPoint() bool GoToMeet::entryPoint() { - ROS_INFO_STREAM("Going to meet at"<goToPoint(current_meeting_location, false); + ROS_INFO_STREAM("Going to meet at"<goToPoint(temp_point, false); return true; } @@ -197,7 +205,7 @@ void Meet::step() void Meet::exitPoint() { - current_meeting_location = next_meeting_location; + setCurrAsNextMeeting(); // get time estimate from move_base_interface // set exploration_duration accordingly From bc43111c68844b28ff8d505ed403deb4cd6cc245 Mon Sep 17 00:00:00 2001 From: Ashay Aswale Date: Mon, 6 Dec 2021 15:07:43 -0500 Subject: [PATCH 05/12] connection checks and publishes --- mdis_state_machine/CMakeLists.txt | 15 ++++ mdis_state_machine/msg/Connection.msg | 2 + mdis_state_machine/src/connection_check.cpp | 87 +++++++++++++++++++++ 3 files changed, 104 insertions(+) create mode 100644 mdis_state_machine/msg/Connection.msg create mode 100644 mdis_state_machine/src/connection_check.cpp diff --git a/mdis_state_machine/CMakeLists.txt b/mdis_state_machine/CMakeLists.txt index 03f9d92..e2cfc22 100644 --- a/mdis_state_machine/CMakeLists.txt +++ b/mdis_state_machine/CMakeLists.txt @@ -17,6 +17,16 @@ find_package(catkin REQUIRED COMPONENTS tf ) +add_message_files( + FILES + Connection.msg +) + +generate_messages( + DEPENDENCIES + std_msgs +) + catkin_package( INCLUDE_DIRS include @@ -70,6 +80,11 @@ 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} diff --git a/mdis_state_machine/msg/Connection.msg b/mdis_state_machine/msg/Connection.msg new file mode 100644 index 0000000..9f65580 --- /dev/null +++ b/mdis_state_machine/msg/Connection.msg @@ -0,0 +1,2 @@ +std_msgs/String[] connection_between +float32 distance \ No newline at end of file diff --git a/mdis_state_machine/src/connection_check.cpp b/mdis_state_machine/src/connection_check.cpp new file mode 100644 index 0000000..de317f1 --- /dev/null +++ b/mdis_state_machine/src/connection_check.cpp @@ -0,0 +1,87 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" + +#include +#include + +#include + +const int MAX_ATTEMPTS=5; +const float DIST_THRESHOLD=1.0; + +geometry_msgs::Point getRobotCurrentPose(const std::string& robot_name, tf::TransformListener& tf_listener) +{ + int attempt = 0; + bool not_found_tf = true; + tf::StampedTransform transform; + geometry_msgs::Point result_transform; + + while (attempt++ < MAX_ATTEMPTS && not_found_tf && ros::ok()){ + try{ + tf_listener.lookupTransform("/"+robot_name+"/map", "/"+robot_name+"/base_footprint", + ros::Time(0), transform); + + not_found_tf = false; + } + catch (tf::TransformException ex){ + ROS_WARN("%s",ex.what()); + ros::Duration(0.10).sleep(); + } + } + + if(not_found_tf) + ROS_ERROR("Could not find the transform after %i attempts", MAX_ATTEMPTS); + else + { + result_transform.x = transform.getOrigin().x(); + result_transform.y = transform.getOrigin().y(); + } + + return result_transform; +} + +float getDistanceOfPoints(const geometry_msgs::Point &pt_1, const geometry_msgs::Point &pt_2) +{ + float x = pt_1.x-pt_2.x; + float y = pt_1.y-pt_2.y; + return std::sqrt(x*x+y*y); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "connection_check"); + ros::NodeHandle nh; + if (argc != 4 && argc != 2) + { + ROS_ERROR("This node must be launched with number of robots as argument"); + return 0; + } + int number_of_robots = std::stoi(argv[1]); + tf::TransformListener tf_listener; + ros::Publisher conn_pub = nh.advertise("connection_check", 1000); + + mdis_state_machine::Connection conn_msg; + conn_msg.connection_between.resize(2); + while(ros::ok()) + { + for (int i=0; i Date: Mon, 6 Dec 2021 18:37:08 -0500 Subject: [PATCH 06/12] Working explorer state machine --- .../include/robot_state_machine.h | 32 +++++++++++++- mdis_state_machine/include/team_scheduler.h | 2 +- mdis_state_machine/src/connection_check.cpp | 8 ++-- .../src/robot_state_machine.cpp | 44 +++++++++++++++---- mdis_state_machine/src/team_scheduler.cpp | 20 ++++++--- 5 files changed, 86 insertions(+), 20 deletions(-) diff --git a/mdis_state_machine/include/robot_state_machine.h b/mdis_state_machine/include/robot_state_machine.h index f72d516..cf56230 100644 --- a/mdis_state_machine/include/robot_state_machine.h +++ b/mdis_state_machine/include/robot_state_machine.h @@ -2,6 +2,7 @@ #include #include +#include enum TEAM_STATES{ IDLE, @@ -37,12 +38,23 @@ class RobotState { virtual TEAM_STATES transition() = 0; + void setParent(const std::string& name) + { + parent_robot_name = name; + } + + void setChild(const std::string& name) + { + child_robot_name = name; + } - static float curr_meet_x, curr_meet_y, next_meet_x, next_meet_y; protected: uint64_t m_unId; std::string m_strName; + std::string robot_name; + static float curr_meet_x, curr_meet_y, next_meet_x, next_meet_y; + static std::string parent_robot_name, child_robot_name; bool is_explorer; bool meeting_started, go_for_exploration; @@ -132,7 +144,9 @@ class Explore: public RobotState{ class GoToMeet: public RobotState{ public: - GoToMeet(ros::NodeHandle &nh):RobotState(GO_TO_MEET, "GoToMeet", nh){} + GoToMeet(ros::NodeHandle &nh):RobotState(GO_TO_MEET, "GoToMeet", nh){ + conn_sub = nh.subscribe("/connection_check", 1000, &GoToMeet::connCB, this); + } bool isDone() override ; TEAM_STATES transition() override; @@ -140,6 +154,20 @@ class GoToMeet: public RobotState{ bool entryPoint() override; void step() override; void exitPoint() override; + +private: + bool connected; + std::string conn_robot; + ros::Subscriber conn_sub; + + inline bool isConnDirectRelated() + { + bool conn_parent = (conn_robot == parent_robot_name) && (conn_robot != ""); + bool conn_child = (conn_robot == child_robot_name) && (conn_robot != ""); + return ((conn_parent) || (conn_child)); + } + + void connCB(const mdis_state_machine::Connection::ConstPtr msg); }; diff --git a/mdis_state_machine/include/team_scheduler.h b/mdis_state_machine/include/team_scheduler.h index 3de5eff..ef00b55 100644 --- a/mdis_state_machine/include/team_scheduler.h +++ b/mdis_state_machine/include/team_scheduler.h @@ -5,7 +5,7 @@ class TeamScheduler{ public: - TeamScheduler(ros::NodeHandle &nh); + TeamScheduler(ros::NodeHandle &nh, const std::string& parent_name, const std::string& child_name); ~TeamScheduler(); void exec(); diff --git a/mdis_state_machine/src/connection_check.cpp b/mdis_state_machine/src/connection_check.cpp index de317f1..51f0348 100644 --- a/mdis_state_machine/src/connection_check.cpp +++ b/mdis_state_machine/src/connection_check.cpp @@ -60,10 +60,10 @@ int main(int argc, char** argv) tf::TransformListener tf_listener; ros::Publisher conn_pub = nh.advertise("connection_check", 1000); - mdis_state_machine::Connection conn_msg; - conn_msg.connection_between.resize(2); while(ros::ok()) { + mdis_state_machine::Connection conn_msg; + conn_msg.connection_between.resize(2); for (int i=0; igoToPoint(temp_point, false); + // Giving it some time to reflect + ros::Duration(1).sleep(); return true; } bool GoToMeet::isDone() { - // Connection established - - // Temporary case - ros::Duration(20).sleep(); - return true; + if(connected) + { + if(isConnDirectRelated()) + { + ROS_INFO("Robot is connected to the party of interest"); + return true; + } + } + return false; } TEAM_STATES GoToMeet::transition() { if(isDone()) + { return MEET; - else GO_TO_MEET; + } + else + return GO_TO_MEET; } void GoToMeet::step() { - ROS_INFO("Step for GoToMeet"); + ROS_INFO_THROTTLE(10,"Step for GoToMeet"); } void GoToMeet::exitPoint() @@ -175,6 +189,19 @@ void GoToMeet::exitPoint() } +void GoToMeet::connCB(const mdis_state_machine::Connection::ConstPtr msg) +{ + connected = false; + for(int i = 0; iconnection_between.size(); i++) + { + if(robot_name == msg->connection_between.at(i).data) + { + int j = 1 ? i==0 : 0; + connected = true; + conn_robot = msg->connection_between.at(j).data; + } + } +} //////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////// M E E T S T A T E C L A S S //////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -205,6 +232,7 @@ void Meet::step() void Meet::exitPoint() { + // Set this only if the meeting was successful setCurrAsNextMeeting(); // get time estimate from move_base_interface diff --git a/mdis_state_machine/src/team_scheduler.cpp b/mdis_state_machine/src/team_scheduler.cpp index 2470c7e..f574856 100644 --- a/mdis_state_machine/src/team_scheduler.cpp +++ b/mdis_state_machine/src/team_scheduler.cpp @@ -1,9 +1,11 @@ #include -TeamScheduler::TeamScheduler(ros::NodeHandle &nh) +TeamScheduler::TeamScheduler(ros::NodeHandle &nh, const std::string& parent_name, const std::string& child_name) { addStates(nh); - setInitialState(IDLE); + setInitialState(GO_TO_EXPLORE); + current_state_ptr->setParent(parent_name); + current_state_ptr->setChild(child_name); } TeamScheduler::~TeamScheduler() @@ -119,10 +121,18 @@ int main(int argc, char** argv) { ros::init(argc, argv, "state_machine"); ros::NodeHandle nh; + + // if (argc != 4 && argc != 2) + // { + // ROS_ERROR("This node must be launched with number of robots as argument"); + // return 0; + // } - ROS_INFO("One"); - TeamScheduler team(nh); - ROS_INFO("exec"); + ROS_WARN("Argument checking is turned off. Please verify if the arguments are parent robot name and child robot name (if applicable)"); + std::string parent_name = argv[1], child_name; + // child_name = argv[2]; + TeamScheduler team(nh, parent_name, child_name); + team.exec(); return 0; From 3f6112d2879b0e6f7721785943b69129b4404e7f Mon Sep 17 00:00:00 2001 From: Ashay Aswale Date: Mon, 6 Dec 2021 22:14:05 -0500 Subject: [PATCH 07/12] Explorer works on its own, doesn't communicate next location --- .../include/robot_state_machine.h | 39 +++++++++ mdis_state_machine/include/team_scheduler.h | 2 +- .../src/robot_state_machine.cpp | 79 ++++++++++++++++++- mdis_state_machine/src/team_scheduler.cpp | 23 ++++-- 4 files changed, 134 insertions(+), 9 deletions(-) diff --git a/mdis_state_machine/include/robot_state_machine.h b/mdis_state_machine/include/robot_state_machine.h index cf56230..a041d4d 100644 --- a/mdis_state_machine/include/robot_state_machine.h +++ b/mdis_state_machine/include/robot_state_machine.h @@ -4,6 +4,12 @@ #include #include +enum ROLE{ + RELAY, + EXPLORER, + RELAY_BETN_ROBOTS, +}; + enum TEAM_STATES{ IDLE, GO_TO_EXPLORE, @@ -48,6 +54,10 @@ class RobotState { child_robot_name = name; } + void setRobotRole(ROLE role) + { + robot_role = role; + } protected: uint64_t m_unId; @@ -55,6 +65,9 @@ class RobotState { std::string robot_name; static float curr_meet_x, curr_meet_y, next_meet_x, next_meet_y; static std::string parent_robot_name, child_robot_name; + static ROLE robot_role; + + geometry_msgs::Point data_dump_location; bool is_explorer; bool meeting_started, go_for_exploration; @@ -184,4 +197,30 @@ class Meet: public RobotState{ }; +class GoToDumpData: public RobotState{ +public: + GoToDumpData(ros::NodeHandle &nh):RobotState(GO_TO_DUMP_DATA, "GoToDumpData", nh){} + bool isDone() override ; + + TEAM_STATES transition() override; + + bool entryPoint() override; + void step() override; + void exitPoint() override; +}; + + +class DumpData: public RobotState{ +public: + DumpData(ros::NodeHandle &nh):RobotState(DUMP_DATA, "DumpData", nh){} + bool isDone() override ; + + TEAM_STATES transition() override; + + bool entryPoint() override; + void step() override; + void exitPoint() override; +}; + + diff --git a/mdis_state_machine/include/team_scheduler.h b/mdis_state_machine/include/team_scheduler.h index ef00b55..f767929 100644 --- a/mdis_state_machine/include/team_scheduler.h +++ b/mdis_state_machine/include/team_scheduler.h @@ -5,7 +5,7 @@ class TeamScheduler{ public: - TeamScheduler(ros::NodeHandle &nh, const std::string& parent_name, const std::string& child_name); + TeamScheduler(ros::NodeHandle &nh, ROLE role, const std::string& parent_name, const std::string& child_name); ~TeamScheduler(); void exec(); diff --git a/mdis_state_machine/src/robot_state_machine.cpp b/mdis_state_machine/src/robot_state_machine.cpp index a63cb18..b9c99fe 100644 --- a/mdis_state_machine/src/robot_state_machine.cpp +++ b/mdis_state_machine/src/robot_state_machine.cpp @@ -8,6 +8,8 @@ float RobotState::next_meet_y = 0.0; std::string RobotState::parent_robot_name = ""; std::string RobotState::child_robot_name = ""; +ROLE RobotState::robot_role; + RobotState::RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHandle &nh) : // m_pcTeam(nullptr), m_unId(un_id), m_strName(str_name) @@ -21,6 +23,8 @@ RobotState::RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHan next_meet_x = -28; next_meet_y = -6; + data_dump_location.x = -31; + data_dump_location.y = -10; } @@ -220,7 +224,14 @@ bool Meet::isDone() TEAM_STATES Meet::transition() { if(isDone()) - return GO_TO_EXPLORE; + { + if (robot_role == EXPLORER) + return GO_TO_EXPLORE; + else if(robot_role == RELAY) + return GO_TO_DUMP_DATA; + // else if(robot_role == RELAY_BETN_ROBOTS) + // return GO_TO_MEET; + } else return MEET; } @@ -238,3 +249,69 @@ void Meet::exitPoint() // get time estimate from move_base_interface // set exploration_duration accordingly } + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////// G O T O D U M P S T A T E C L A S S //////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +bool GoToDumpData::entryPoint() +{ + return true; +} + +bool GoToDumpData::isDone() +{ + ROS_INFO_STREAM("Going to Dump Data at"<goToPoint(data_dump_location, true); + return true; +} + +TEAM_STATES GoToDumpData::transition() +{ + if(isDone()) + return DUMP_DATA; + else + return GO_TO_DUMP_DATA; +} + +void GoToDumpData::step() +{ + ROS_INFO_THROTTLE(10, "Going to dump data"); +} + +void GoToDumpData::exitPoint() +{ + explore_interface->stopRobot(); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////// D U M P S T A T E C L A S S //////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +bool DumpData::entryPoint() +{ + return true; +} + +bool DumpData::isDone() +{ + // Data sync done + return true; +} + +TEAM_STATES DumpData::transition() +{ + if(isDone()) + return GO_TO_MEET; + else + return DUMP_DATA; +} + +void DumpData::step() +{ + ROS_INFO("Step for DumpData"); +} + +void DumpData::exitPoint() +{ +} diff --git a/mdis_state_machine/src/team_scheduler.cpp b/mdis_state_machine/src/team_scheduler.cpp index f574856..2027f40 100644 --- a/mdis_state_machine/src/team_scheduler.cpp +++ b/mdis_state_machine/src/team_scheduler.cpp @@ -1,11 +1,15 @@ #include -TeamScheduler::TeamScheduler(ros::NodeHandle &nh, const std::string& parent_name, const std::string& child_name) +TeamScheduler::TeamScheduler(ros::NodeHandle &nh, ROLE role, const std::string& parent_name, const std::string& child_name) { addStates(nh); - setInitialState(GO_TO_EXPLORE); + if (role == EXPLORER) + setInitialState(GO_TO_EXPLORE); + else if (role == RELAY) + setInitialState(GO_TO_MEET); current_state_ptr->setParent(parent_name); current_state_ptr->setChild(child_name); + current_state_ptr->setRobotRole(role); } TeamScheduler::~TeamScheduler() @@ -25,6 +29,8 @@ void TeamScheduler::addStates(ros::NodeHandle &nh) addState(new Explore(nh)); addState(new GoToMeet(nh)); addState(new Meet(nh)); + addState(new GoToDumpData(nh)); + addState(new DumpData(nh)); } void TeamScheduler::addState(RobotState* pc_state) { @@ -128,12 +134,15 @@ int main(int argc, char** argv) // return 0; // } - ROS_WARN("Argument checking is turned off. Please verify if the arguments are parent robot name and child robot name (if applicable)"); - std::string parent_name = argv[1], child_name; - // child_name = argv[2]; - TeamScheduler team(nh, parent_name, child_name); + ROS_WARN("Argument checking is turned off. Please verify if the arguments are: Role, parent_robot_name and child_robot_name (if applicable)"); + + ROLE role = (ROLE)(std::stoi(argv[1])); + std::string parent_name = argv[2], child_name; + if(role != EXPLORER) + child_name = argv[3]; + TeamScheduler team(nh, role, parent_name, child_name); team.exec(); - return 0; + return 0; } From 6b6be7ddd5bcc548bc1ed7745c47d769d169d5f9 Mon Sep 17 00:00:00 2001 From: Ashay Aswale Date: Tue, 7 Dec 2021 01:04:31 -0500 Subject: [PATCH 08/12] working communication of relay and explorer --- mdis_state_machine/CMakeLists.txt | 2 + .../include/robot_state_machine.h | 36 +++++++---- mdis_state_machine/msg/DataCommunication.msg | 2 + .../src/robot_state_machine.cpp | 63 ++++++++++++++++++- mdis_state_machine/src/team_scheduler.cpp | 4 +- 5 files changed, 92 insertions(+), 15 deletions(-) create mode 100644 mdis_state_machine/msg/DataCommunication.msg diff --git a/mdis_state_machine/CMakeLists.txt b/mdis_state_machine/CMakeLists.txt index e2cfc22..5ab04d4 100644 --- a/mdis_state_machine/CMakeLists.txt +++ b/mdis_state_machine/CMakeLists.txt @@ -20,11 +20,13 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( FILES Connection.msg + DataCommunication.msg ) generate_messages( DEPENDENCIES std_msgs + geometry_msgs ) diff --git a/mdis_state_machine/include/robot_state_machine.h b/mdis_state_machine/include/robot_state_machine.h index a041d4d..cdeca6e 100644 --- a/mdis_state_machine/include/robot_state_machine.h +++ b/mdis_state_machine/include/robot_state_machine.h @@ -3,7 +3,7 @@ #include #include #include - +#include enum ROLE{ RELAY, EXPLORER, @@ -77,12 +77,12 @@ class RobotState { ros::Duration time_until_next_meeting = ros::Duration(30.0); - void setCurrentMeetingLocation(const geometry_msgs::Point& meeting) + void setCurrentMeetingPoint(const geometry_msgs::Point& meeting) { curr_meet_x = meeting.x; curr_meet_y = meeting.y; } - void setNextMeetingLocation(const geometry_msgs::Point& meeting) + void setNextMeetingPoint(const geometry_msgs::Point& meeting) { next_meet_x = meeting.x; next_meet_y = meeting.y; @@ -106,6 +106,13 @@ class RobotState { curr_meet_x = next_meet_x; curr_meet_y = next_meet_y; } + inline bool isConnDirectRelated(const std::string& conn_robot) + { + bool conn_parent = (conn_robot == parent_robot_name) && (conn_robot != ""); + bool conn_child = (conn_robot == child_robot_name) && (conn_robot != ""); + return ((conn_parent) || (conn_child)); + } + }; class Idle: public RobotState{ @@ -173,20 +180,16 @@ class GoToMeet: public RobotState{ std::string conn_robot; ros::Subscriber conn_sub; - inline bool isConnDirectRelated() - { - bool conn_parent = (conn_robot == parent_robot_name) && (conn_robot != ""); - bool conn_child = (conn_robot == child_robot_name) && (conn_robot != ""); - return ((conn_parent) || (conn_child)); - } - void connCB(const mdis_state_machine::Connection::ConstPtr msg); }; class Meet: public RobotState{ public: - Meet(ros::NodeHandle &nh):RobotState(MEET, "Meet", nh){} + Meet(ros::NodeHandle &nh):RobotState(MEET, "Meet", nh){ + meeting_data_pub = nh.advertise("/data_communication", 1000); + meeting_data_sub = nh.subscribe("/data_communication", 1000, &Meet::nextMeetingLocationCB, this); + } bool isDone() override ; TEAM_STATES transition() override; @@ -194,6 +197,17 @@ class Meet: public RobotState{ bool entryPoint() override; void step() override; void exitPoint() override; + +private: + ros::Publisher meeting_data_pub; + ros::Subscriber meeting_data_sub; + + bool data_received; + + void publishNextMeetingLocation(); + void nextMeetingLocationCB(const mdis_state_machine::DataCommunication::ConstPtr msg); + void getNextMeetingLocationFromCallback(); + geometry_msgs::Point buffer_next_location; }; diff --git a/mdis_state_machine/msg/DataCommunication.msg b/mdis_state_machine/msg/DataCommunication.msg new file mode 100644 index 0000000..796be33 --- /dev/null +++ b/mdis_state_machine/msg/DataCommunication.msg @@ -0,0 +1,2 @@ +std_msgs/String[] connection_between +geometry_msgs/Point next_meeting_point \ No newline at end of file diff --git a/mdis_state_machine/src/robot_state_machine.cpp b/mdis_state_machine/src/robot_state_machine.cpp index b9c99fe..d65f2ef 100644 --- a/mdis_state_machine/src/robot_state_machine.cpp +++ b/mdis_state_machine/src/robot_state_machine.cpp @@ -136,7 +136,7 @@ void Explore::exitPoint() // BUG!!!! // Values are being changed only for the class Explore // Need to change values for all - setNextMeetingLocation(explore_interface->getRobotCurrentPose().pose.position); + setNextMeetingPoint(explore_interface->getRobotCurrentPose().pose.position); ROS_INFO_STREAM("Next meeting"<connection_between.size(); i++) + { + if(robot_name == msg->connection_between.at(i).data) + { + int j = 1 ? i==0 : 0; + conn_robot = msg->connection_between.at(j).data; + } + } + if(isConnDirectRelated(conn_robot)) + { + buffer_next_location = msg->next_meeting_point; + data_received = true; + } +} //////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////// G O T O D U M P S T A T E C L A S S //////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/mdis_state_machine/src/team_scheduler.cpp b/mdis_state_machine/src/team_scheduler.cpp index 2027f40..6c90164 100644 --- a/mdis_state_machine/src/team_scheduler.cpp +++ b/mdis_state_machine/src/team_scheduler.cpp @@ -135,11 +135,13 @@ int main(int argc, char** argv) // } ROS_WARN("Argument checking is turned off. Please verify if the arguments are: Role, parent_robot_name and child_robot_name (if applicable)"); - ROLE role = (ROLE)(std::stoi(argv[1])); std::string parent_name = argv[2], child_name; if(role != EXPLORER) child_name = argv[3]; + + if(role == RELAY) + ros::Duration(20).sleep(); TeamScheduler team(nh, role, parent_name, child_name); team.exec(); From 550296925205109a8c4f25621e3086c4883a0a56 Mon Sep 17 00:00:00 2001 From: Ashay Aswale Date: Thu, 9 Dec 2021 14:15:41 -0500 Subject: [PATCH 09/12] pending commit for time prediction --- .../include/move_base_interface.h | 17 +++++++++++++++++ .../include/robot_state_machine.h | 3 ++- mdis_state_machine/src/connection_check.cpp | 2 +- mdis_state_machine/src/move_base_interface.cpp | 16 +++++++++++++++- mdis_state_machine/src/move_base_test.cpp | 15 +++++++++------ mdis_state_machine/src/robot_state_machine.cpp | 16 ++++++++++++++++ 6 files changed, 60 insertions(+), 9 deletions(-) diff --git a/mdis_state_machine/include/move_base_interface.h b/mdis_state_machine/include/move_base_interface.h index ae4aab3..242134e 100644 --- a/mdis_state_machine/include/move_base_interface.h +++ b/mdis_state_machine/include/move_base_interface.h @@ -47,6 +47,22 @@ class MoveBaseInterface */ 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 * @@ -111,6 +127,7 @@ class MoveBaseInterface 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); diff --git a/mdis_state_machine/include/robot_state_machine.h b/mdis_state_machine/include/robot_state_machine.h index cdeca6e..20f6d53 100644 --- a/mdis_state_machine/include/robot_state_machine.h +++ b/mdis_state_machine/include/robot_state_machine.h @@ -64,6 +64,7 @@ class RobotState { std::string m_strName; std::string robot_name; static float curr_meet_x, curr_meet_y, next_meet_x, next_meet_y; + static float time_for_exploration; static std::string parent_robot_name, child_robot_name; static ROLE robot_role; @@ -74,7 +75,6 @@ class RobotState { MoveBaseInterface *explore_interface; - ros::Duration time_until_next_meeting = ros::Duration(30.0); void setCurrentMeetingPoint(const geometry_msgs::Point& meeting) @@ -207,6 +207,7 @@ class Meet: public RobotState{ void publishNextMeetingLocation(); void nextMeetingLocationCB(const mdis_state_machine::DataCommunication::ConstPtr msg); void getNextMeetingLocationFromCallback(); + void setExplorationTime(); geometry_msgs::Point buffer_next_location; }; diff --git a/mdis_state_machine/src/connection_check.cpp b/mdis_state_machine/src/connection_check.cpp index 51f0348..ce1dc02 100644 --- a/mdis_state_machine/src/connection_check.cpp +++ b/mdis_state_machine/src/connection_check.cpp @@ -7,7 +7,7 @@ #include const int MAX_ATTEMPTS=5; -const float DIST_THRESHOLD=1.0; +const float DIST_THRESHOLD=3.0; geometry_msgs::Point getRobotCurrentPose(const std::string& robot_name, tf::TransformListener& tf_listener) { diff --git a/mdis_state_machine/src/move_base_interface.cpp b/mdis_state_machine/src/move_base_interface.cpp index 5c64907..26f8402 100644 --- a/mdis_state_machine/src/move_base_interface.cpp +++ b/mdis_state_machine/src/move_base_interface.cpp @@ -166,4 +166,18 @@ geometry_msgs::PoseStamped MoveBaseInterface::transformTf2msg(const tf::StampedT msg.pose.orientation.w = transform.getRotation().w(); return msg; -} \ No newline at end of file +} + +float MoveBaseInterface::getTimePredictionForTravel(geometry_msgs::Point &goal) +{ + float distance = getDistancePrediction(goal); + float time = distance/ROBOT_SPEED; + return time; +} + +float MoveBaseInterface::getTimePredictionForTravel(geometry_msgs::Point &start_point, geometry_msgs::Point &end_point) +{ + float distance = getDistancePrediction(start_point, end_point); + float time = distance/ROBOT_SPEED; + return time; +} diff --git a/mdis_state_machine/src/move_base_test.cpp b/mdis_state_machine/src/move_base_test.cpp index a318958..ee5b5e6 100644 --- a/mdis_state_machine/src/move_base_test.cpp +++ b/mdis_state_machine/src/move_base_test.cpp @@ -9,14 +9,17 @@ int main(int argc, char** argv) geometry_msgs::Point point_1, point_2; point_1.x = -31; point_1.y = -8; - point_2.x = -31; - point_2.y = -6; - float dist = explore.getDistancePrediction(point_2); - ROS_INFO_STREAM("Distance: "< time_until_next_meeting; } @@ -249,6 +252,7 @@ void Meet::exitPoint() if(robot_role == EXPLORER) { publishNextMeetingLocation(); + setExplorationTime(); setCurrAsNextMeeting(); } else @@ -259,6 +263,18 @@ void Meet::exitPoint() // set exploration_duration accordingly } +void Meet::setExplorationTime() +{ + geometry_msgs::Point curr_location = explore_interface->getRobotCurrentPose().pose.position; + geometry_msgs::Point temp_next_location = getNextMeetingPoint(); + float time_to_dump = explore_interface->getTimePredictionForTravel(curr_location, data_dump_location); + float time_to_meet_after = explore_interface->getTimePredictionForTravel(data_dump_location, temp_next_location); + float time_to_exploration_site = explore_interface->getTimePredictionForTravel(curr_location, temp_next_location); + float total_time = time_to_dump+time_to_meet_after; + + time_for_exploration = (total_time-time_to_exploration_site)/2; +} + void Meet::publishNextMeetingLocation() { mdis_state_machine::DataCommunication data; From a793680ae1f12dd47e0693cfbd0f443f793b21fc Mon Sep 17 00:00:00 2001 From: AshayAswale Date: Fri, 4 Feb 2022 16:14:57 -0500 Subject: [PATCH 10/12] Updated commit hash for pausing exploration --- turtlebot3_simulations | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/turtlebot3_simulations b/turtlebot3_simulations index 412a5cb..7efe079 160000 --- a/turtlebot3_simulations +++ b/turtlebot3_simulations @@ -1 +1 @@ -Subproject commit 412a5cbc2b4db0632b93b478b46474b90707ffe1 +Subproject commit 7efe07903f859b126a86c29db27fec48aa5713dd From bbb73d4d395b2d50b7147bdf5ad9a5e93faec55e Mon Sep 17 00:00:00 2001 From: AshayAswale Date: Thu, 17 Feb 2022 16:28:50 -0500 Subject: [PATCH 11/12] Added ReadMe file for the state machine flow --- mdis_state_machine/StateMachineReadMe.md | 63 ++++++++++++++++++++++++ 1 file changed, 63 insertions(+) create mode 100644 mdis_state_machine/StateMachineReadMe.md diff --git a/mdis_state_machine/StateMachineReadMe.md b/mdis_state_machine/StateMachineReadMe.md new file mode 100644 index 0000000..7b89da8 --- /dev/null +++ b/mdis_state_machine/StateMachineReadMe.md @@ -0,0 +1,63 @@ +# 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 `/connection_check` + * Message type is: `mdis_state_machine::Connection` +* 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 \ No newline at end of file From 6d53a3a66690b99d9fb470f5e7bd0d7d74cbca3e Mon Sep 17 00:00:00 2001 From: AshayAswale Date: Thu, 17 Feb 2022 20:42:28 -0500 Subject: [PATCH 12/12] Modifications as requested by #18 --- mdis_state_machine/CMakeLists.txt | 1 + mdis_state_machine/StateMachineReadMe.md | 3 + .../include/robot_state_machine.h | 54 ++++-- mdis_state_machine/include/team_scheduler.h | 4 +- .../launch/test_state_machine.launch | 11 ++ mdis_state_machine/msg/RobotsState.msg | 2 + .../src/robot_state_machine.cpp | 157 ++++++++++++++++-- mdis_state_machine/src/team_scheduler.cpp | 51 ++++-- 8 files changed, 235 insertions(+), 48 deletions(-) create mode 100644 mdis_state_machine/launch/test_state_machine.launch create mode 100644 mdis_state_machine/msg/RobotsState.msg diff --git a/mdis_state_machine/CMakeLists.txt b/mdis_state_machine/CMakeLists.txt index 5ab04d4..334b1da 100644 --- a/mdis_state_machine/CMakeLists.txt +++ b/mdis_state_machine/CMakeLists.txt @@ -21,6 +21,7 @@ add_message_files( FILES Connection.msg DataCommunication.msg + RobotsState.msg ) generate_messages( diff --git a/mdis_state_machine/StateMachineReadMe.md b/mdis_state_machine/StateMachineReadMe.md index 7b89da8..8661d48 100644 --- a/mdis_state_machine/StateMachineReadMe.md +++ b/mdis_state_machine/StateMachineReadMe.md @@ -38,6 +38,9 @@ ___ * In test cases, the tester will have to emulate the connection by publishing the requied message over rostopic * Currently, topic is `/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) diff --git a/mdis_state_machine/include/robot_state_machine.h b/mdis_state_machine/include/robot_state_machine.h index 20f6d53..3a08e25 100644 --- a/mdis_state_machine/include/robot_state_machine.h +++ b/mdis_state_machine/include/robot_state_machine.h @@ -2,6 +2,8 @@ #include #include +#include +#include #include #include enum ROLE{ @@ -24,7 +26,7 @@ class RobotState { public: - RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHandle &nh); + RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHandle &nh, bool testing); ~RobotState() { @@ -66,8 +68,11 @@ class RobotState { static float curr_meet_x, curr_meet_y, next_meet_x, next_meet_y; static float time_for_exploration; static std::string parent_robot_name, child_robot_name; + static bool testing_mode; static ROLE robot_role; + int testing_waiting_time = 1; + geometry_msgs::Point data_dump_location; bool is_explorer; @@ -117,7 +122,7 @@ class RobotState { class Idle: public RobotState{ public: - Idle(ros::NodeHandle &nh):RobotState(IDLE, "Idle", nh){} + Idle(ros::NodeHandle &nh, bool testing):RobotState(IDLE, "Idle", nh, testing){} bool isDone() override ; TEAM_STATES transition() override; @@ -132,7 +137,9 @@ class Idle: public RobotState{ class GoToExplore: public RobotState{ public: - GoToExplore(ros::NodeHandle &nh):RobotState(GO_TO_EXPLORE, "GoToExplore", nh){} + GoToExplore(ros::NodeHandle &nh, bool testing):RobotState(GO_TO_EXPLORE, "GoToExplore", nh, testing){ + robot_state_pub = nh.advertise("robots_state", 1000); + } bool isDone() override ; TEAM_STATES transition() override; @@ -140,13 +147,18 @@ class GoToExplore: public RobotState{ bool entryPoint() override; void step() override; void exitPoint() override; + +private: + ros::Publisher robot_state_pub; + mdis_state_machine::RobotsState state_pub_data; }; class Explore: public RobotState{ public: - Explore(ros::NodeHandle &nh):RobotState(EXPLORE, "Explore", nh){ - pause_exploration_pub = nh.advertise("explore/pause_exploration", 1000); + Explore(ros::NodeHandle &nh, bool testing):RobotState(EXPLORE, "Explore", nh, testing){ + pause_exploration_pub = nh.advertise("explore/pause_exploration", 1000); + robot_state_pub = nh.advertise("robots_state", 1000); } bool isDone() override ; @@ -158,14 +170,17 @@ class Explore: public RobotState{ private: ros::Publisher pause_exploration_pub; + ros::Publisher robot_state_pub; + mdis_state_machine::RobotsState state_pub_data; ros::Time starting_time; }; class GoToMeet: public RobotState{ public: - GoToMeet(ros::NodeHandle &nh):RobotState(GO_TO_MEET, "GoToMeet", nh){ - conn_sub = nh.subscribe("/connection_check", 1000, &GoToMeet::connCB, this); + GoToMeet(ros::NodeHandle &nh, bool testing):RobotState(GO_TO_MEET, "GoToMeet", nh, testing){ + conn_sub = nh.subscribe("/connection_check", 1000, &GoToMeet::connCB, this); + robot_state_pub = nh.advertise("robots_state", 1000); } bool isDone() override ; @@ -179,6 +194,8 @@ class GoToMeet: public RobotState{ bool connected; std::string conn_robot; ros::Subscriber conn_sub; + ros::Publisher robot_state_pub; + mdis_state_machine::RobotsState state_pub_data; void connCB(const mdis_state_machine::Connection::ConstPtr msg); }; @@ -186,9 +203,10 @@ class GoToMeet: public RobotState{ class Meet: public RobotState{ public: - Meet(ros::NodeHandle &nh):RobotState(MEET, "Meet", nh){ + Meet(ros::NodeHandle &nh, bool testing):RobotState(MEET, "Meet", nh, testing){ meeting_data_pub = nh.advertise("/data_communication", 1000); - meeting_data_sub = nh.subscribe("/data_communication", 1000, &Meet::nextMeetingLocationCB, this); + meeting_data_sub = nh.subscribe("/data_communication", 1000, &Meet::nextMeetingLocationCB, this); + robot_state_pub = nh.advertise("robots_state", 1000); } bool isDone() override ; @@ -200,6 +218,8 @@ class Meet: public RobotState{ private: ros::Publisher meeting_data_pub; + ros::Publisher robot_state_pub; + mdis_state_machine::RobotsState state_pub_data; ros::Subscriber meeting_data_sub; bool data_received; @@ -214,7 +234,9 @@ class Meet: public RobotState{ class GoToDumpData: public RobotState{ public: - GoToDumpData(ros::NodeHandle &nh):RobotState(GO_TO_DUMP_DATA, "GoToDumpData", nh){} + GoToDumpData(ros::NodeHandle &nh, bool testing):RobotState(GO_TO_DUMP_DATA, "GoToDumpData", nh, testing){ + robot_state_pub = nh.advertise("robots_state", 1000); + } bool isDone() override ; TEAM_STATES transition() override; @@ -222,12 +244,18 @@ class GoToDumpData: public RobotState{ bool entryPoint() override; void step() override; void exitPoint() override; + +private: + ros::Publisher robot_state_pub; + mdis_state_machine::RobotsState state_pub_data; }; class DumpData: public RobotState{ public: - DumpData(ros::NodeHandle &nh):RobotState(DUMP_DATA, "DumpData", nh){} + DumpData(ros::NodeHandle &nh, bool testing):RobotState(DUMP_DATA, "DumpData", nh, testing){ + robot_state_pub = nh.advertise("robots_state", 1000); + } bool isDone() override ; TEAM_STATES transition() override; @@ -235,6 +263,10 @@ class DumpData: public RobotState{ bool entryPoint() override; void step() override; void exitPoint() override; + +private: + ros::Publisher robot_state_pub; + mdis_state_machine::RobotsState state_pub_data; }; diff --git a/mdis_state_machine/include/team_scheduler.h b/mdis_state_machine/include/team_scheduler.h index f767929..3443d3d 100644 --- a/mdis_state_machine/include/team_scheduler.h +++ b/mdis_state_machine/include/team_scheduler.h @@ -5,7 +5,7 @@ class TeamScheduler{ public: - TeamScheduler(ros::NodeHandle &nh, ROLE role, const std::string& parent_name, const std::string& child_name); + TeamScheduler(ros::NodeHandle &nh, ROLE role, const std::string& parent_name, const std::string& child_name, bool testing); ~TeamScheduler(); void exec(); @@ -16,7 +16,7 @@ class TeamScheduler{ robot_state = state;} private: - void addStates(ros::NodeHandle &nh); + void addStates(ros::NodeHandle &nh, bool testing_mode); void addState(RobotState* RobotStatePtr); TEAM_STATES robot_state; bool new_state_request; diff --git a/mdis_state_machine/launch/test_state_machine.launch b/mdis_state_machine/launch/test_state_machine.launch new file mode 100644 index 0000000..2864bb2 --- /dev/null +++ b/mdis_state_machine/launch/test_state_machine.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/mdis_state_machine/msg/RobotsState.msg b/mdis_state_machine/msg/RobotsState.msg new file mode 100644 index 0000000..ac45e7f --- /dev/null +++ b/mdis_state_machine/msg/RobotsState.msg @@ -0,0 +1,2 @@ +std_msgs/String robot_name +int8 robot_state \ No newline at end of file diff --git a/mdis_state_machine/src/robot_state_machine.cpp b/mdis_state_machine/src/robot_state_machine.cpp index 22c5760..63d699a 100644 --- a/mdis_state_machine/src/robot_state_machine.cpp +++ b/mdis_state_machine/src/robot_state_machine.cpp @@ -9,13 +9,23 @@ float RobotState::time_for_exploration = 30.0; std::string RobotState::parent_robot_name = ""; std::string RobotState::child_robot_name = ""; +bool RobotState::testing_mode = false; ROLE RobotState::robot_role; -RobotState::RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHandle &nh) : +RobotState::RobotState(uint64_t un_id, const std::string& str_name, ros::NodeHandle &nh, bool testing) : // m_pcTeam(nullptr), - m_unId(un_id), m_strName(str_name) + m_unId(un_id), m_strName(str_name) { - explore_interface = new MoveBaseInterface(nh); + if(!testing) + { + explore_interface = new MoveBaseInterface(nh); + } + else + { + ROS_INFO("Testing mode, skipping initializations"); + RobotState::testing_mode = true; + } + robot_name = ros::this_node::getNamespace(); robot_name.erase(robot_name.begin()); @@ -66,13 +76,33 @@ void Idle::exitPoint() bool GoToExplore::entryPoint() { + ROS_INFO("Entering the state GO_TO_EXPLORE"); return true; } bool GoToExplore::isDone() { - ROS_INFO_STREAM("Going to explore at"<goToPoint(temp_point, true); return true; } @@ -87,11 +117,12 @@ TEAM_STATES GoToExplore::transition() void GoToExplore::step() { - ROS_INFO("Step for GoToExplore"); + ROS_INFO("Executing step for GO_TO_EXPLORE"); } void GoToExplore::exitPoint() { + ROS_INFO("Exiting the state GO_TO_EXPLORE"); } @@ -102,12 +133,30 @@ void GoToExplore::exitPoint() bool Explore::entryPoint() { starting_time = ros::Time::now(); + ROS_INFO("Entering the state EXPLORE"); ROS_INFO_STREAM("Exploring for "< time_until_next_meeting; @@ -126,7 +175,7 @@ void Explore::step() std_msgs::Bool msg; msg.data = false; pause_exploration_pub.publish(msg); - ROS_INFO_THROTTLE(10,"Step for Explore"); + ROS_INFO_THROTTLE(10,"Executing step for EXPLORE"); } void Explore::exitPoint() @@ -139,11 +188,14 @@ void Explore::exitPoint() // BUG!!!! // Values are being changed only for the class Explore // Need to change values for all - setNextMeetingPoint(explore_interface->getRobotCurrentPose().pose.position); + if(!RobotState::testing_mode) + { + setNextMeetingPoint(explore_interface->getRobotCurrentPose().pose.position); + explore_interface->stopRobot(); + } - ROS_INFO_STREAM("Next meeting"<stopRobot(); } @@ -153,9 +205,12 @@ void Explore::exitPoint() bool GoToMeet::entryPoint() { + ROS_INFO("Entering the state GO_TO_MEET"); ROS_INFO_STREAM("Going to meet at"<goToPoint(temp_point, false); + connected = false; + if(!RobotState::testing_mode) + explore_interface->goToPoint(temp_point, false); // Giving it some time to reflect ros::Duration(1).sleep(); return true; @@ -187,12 +242,18 @@ TEAM_STATES GoToMeet::transition() void GoToMeet::step() { - ROS_INFO_THROTTLE(10,"Step for GoToMeet"); + state_pub_data.robot_name.data = robot_name; + state_pub_data.robot_state = (int)GO_TO_MEET; + robot_state_pub.publish(state_pub_data); + + ROS_INFO_THROTTLE(10,"Executing the step for GO_TO_MEET"); } void GoToMeet::exitPoint() { - explore_interface->stopRobot(); + ROS_INFO("Exiting the state GO_TO_MEET"); + if(!RobotState::testing_mode) + explore_interface->stopRobot(); } @@ -215,6 +276,7 @@ void GoToMeet::connCB(const mdis_state_machine::Connection::ConstPtr msg) bool Meet::entryPoint() { + ROS_INFO("Entering the state MEET"); data_received = false; return true; } @@ -222,6 +284,19 @@ bool Meet::entryPoint() bool Meet::isDone() { // Data sync done + + if(RobotState::testing_mode) + { + int i = 0; + int rate = 5; + + // Hardcoded publish for 5 seconds + while(i++ < testing_waiting_time*rate) + { + ros::Rate(rate).sleep(); + robot_state_pub.publish(state_pub_data); + } + } return true; } @@ -242,17 +317,22 @@ TEAM_STATES Meet::transition() void Meet::step() { - ROS_INFO("Step for Meet"); + state_pub_data.robot_name.data = robot_name; + state_pub_data.robot_state = (int)MEET; + robot_state_pub.publish(state_pub_data); + + ROS_INFO("Executing the step for MEET"); } void Meet::exitPoint() { - ROS_INFO("Exiting"); + ROS_INFO("Exiting the state MEET"); // Set this only if the meeting was successful if(robot_role == EXPLORER) { publishNextMeetingLocation(); - setExplorationTime(); + if(!RobotState::testing_mode) + setExplorationTime(); setCurrAsNextMeeting(); } else @@ -295,7 +375,7 @@ void Meet::getNextMeetingLocationFromCallback() { // @to-do // THIS NEEDS TO EXIT SOMETIME IF NO CONNECTION IS MADE - while(ros::ok() && !data_received) + while(ros::ok() && (!testing_mode && !data_received)) { ROS_INFO("Waiting for meeting data"); ros::spinOnce(); @@ -329,12 +409,32 @@ void Meet::nextMeetingLocationCB(const mdis_state_machine::DataCommunication::Co bool GoToDumpData::entryPoint() { + ROS_INFO("Entering the state GO_TO_DUMP"); return true; } bool GoToDumpData::isDone() { ROS_INFO_STREAM("Going to Dump Data at"<goToPoint(data_dump_location, true); return true; } @@ -354,7 +454,9 @@ void GoToDumpData::step() void GoToDumpData::exitPoint() { - explore_interface->stopRobot(); + ROS_INFO("Exiting the state GO_TO_DUMP"); + if(!RobotState::testing_mode) + explore_interface->stopRobot(); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -363,12 +465,32 @@ void GoToDumpData::exitPoint() bool DumpData::entryPoint() { + ROS_INFO("Entering the state DUMP_DATA"); return true; } bool DumpData::isDone() { // Data sync done + + state_pub_data.robot_name.data = robot_name; + state_pub_data.robot_state = (int)DUMP_DATA; + + if(RobotState::testing_mode) + { + int i = 0; + int rate = 5; + + // Hardcoded publish for 5 seconds + while(i++ < testing_waiting_time*rate) + { + ros::Rate(rate).sleep(); + robot_state_pub.publish(state_pub_data); + } + + return true; + } + return true; } @@ -387,4 +509,5 @@ void DumpData::step() void DumpData::exitPoint() { + ROS_INFO("Exiting the state DUMP_DATA"); } diff --git a/mdis_state_machine/src/team_scheduler.cpp b/mdis_state_machine/src/team_scheduler.cpp index 6c90164..8bcf9bb 100644 --- a/mdis_state_machine/src/team_scheduler.cpp +++ b/mdis_state_machine/src/team_scheduler.cpp @@ -1,8 +1,8 @@ #include -TeamScheduler::TeamScheduler(ros::NodeHandle &nh, ROLE role, const std::string& parent_name, const std::string& child_name) +TeamScheduler::TeamScheduler(ros::NodeHandle &nh, ROLE role, const std::string& parent_name, const std::string& child_name, bool testing) { - addStates(nh); + addStates(nh, testing); if (role == EXPLORER) setInitialState(GO_TO_EXPLORE); else if (role == RELAY) @@ -22,15 +22,15 @@ TeamScheduler::~TeamScheduler() }); } -void TeamScheduler::addStates(ros::NodeHandle &nh) +void TeamScheduler::addStates(ros::NodeHandle &nh, bool testing_mode) { - addState(new Idle(nh)); - addState(new GoToExplore(nh)); - addState(new Explore(nh)); - addState(new GoToMeet(nh)); - addState(new Meet(nh)); - addState(new GoToDumpData(nh)); - addState(new DumpData(nh)); + addState(new Idle(nh, testing_mode)); + addState(new GoToExplore(nh, testing_mode)); + addState(new Explore(nh, testing_mode)); + addState(new GoToMeet(nh, testing_mode)); + addState(new Meet(nh, testing_mode)); + addState(new GoToDumpData(nh, testing_mode)); + addState(new DumpData(nh, testing_mode)); } void TeamScheduler::addState(RobotState* pc_state) { @@ -134,15 +134,30 @@ int main(int argc, char** argv) // return 0; // } - ROS_WARN("Argument checking is turned off. Please verify if the arguments are: Role, parent_robot_name and child_robot_name (if applicable)"); - ROLE role = (ROLE)(std::stoi(argv[1])); - std::string parent_name = argv[2], child_name; + bool testing = false; + ROLE role; + std::string parent_name, child_name; + + if (std::stoi(argv[1]) < 5) + { + ROS_WARN("Argument checking is turned off. Please verify if the arguments are: Role, parent_robot_name and child_robot_name (if applicable)"); + role = (ROLE)(std::stoi(argv[1])); + } + else + { + ROS_WARN("TESTING MODE ACTIVE!"); + testing = true; + role = std::stoi(argv[1]) == 5 ? EXPLORER : RELAY; + } + + parent_name = argv[2]; if(role != EXPLORER) - child_name = argv[3]; - - if(role == RELAY) - ros::Duration(20).sleep(); - TeamScheduler team(nh, role, parent_name, child_name); + child_name = argv[3]; + + if(role == RELAY && !testing) + ros::Duration(20).sleep(); + + TeamScheduler team(nh, role, parent_name, child_name, testing); team.exec();