State Machine IP Addressing

mdis_state_machine/CMakeLists.txt

@@ -23,7 +23,6 @@ catkin_python_setup()
 
 add_message_files(
   FILES
-  Connection.msg
   DataCommunication.msg
   RobotsState.msg
 )
@@ -92,9 +91,6 @@ add_dependencies(team_scheduler ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXP
 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}

mdis_state_machine/include/robot_state_machine.h

@@ -4,8 +4,9 @@
 #include <std_msgs/Bool.h>
 #include <std_msgs/Int8.h>
 #include <mdis_state_machine/RobotsState.h>
-#include <mdis_state_machine/Connection.h>
 #include <mdis_state_machine/DataCommunication.h>
+#include <coms/nearby.h>
+#include <vector>
 enum ROLE{
   RELAY,
   EXPLORER,
@@ -179,7 +180,7 @@ class Explore: public RobotState{
 class GoToMeet: public RobotState{
 public:
    GoToMeet(ros::NodeHandle &nh, bool testing):RobotState(GO_TO_MEET, "GoToMeet", nh, testing){
-      conn_sub = nh.subscribe(nh.getNamespace() + "/connection_check", 1000, &GoToMeet::connCB, this);     
+      conn_sub = nh.subscribe(nh.getNamespace() + "/nearby", 1000, &GoToMeet::connCB, this);     
       robot_state_pub = nh.advertise<mdis_state_machine::RobotsState>(nh.getNamespace() + "/robots_state", 1000);
    }
    bool isDone() override ;
@@ -192,12 +193,12 @@ class GoToMeet: public RobotState{
 
 private:
    bool connected;
-   std::string conn_robot;
+   std::vector<std::string> conn_robots;
    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);
+   void connCB(const coms::nearby::ConstPtr msg);
 };
 
 

mdis_state_machine/launch/test_state_machine_explorer.launch

@@ -1,7 +1,7 @@
 <launch>
   <!-- The argument 5 is to notify the code that it is in testing mode for the robot Explorer -->
   <group ns = "test_robot_1">
-    <node pkg="mdis_state_machine" type="team_scheduler" name="team_scheduler" args="5 dummy_parent dummy_child" output="screen"/>
+    <node pkg="mdis_state_machine" type="team_scheduler" name="team_scheduler" args="5 192.168.0.2:8765 192.168.0.3:8765" output="screen"/>
   </group>
 
   <!-- The argument 6 is to notify the code that it is in testing mode for the robot Relay -->

mdis_state_machine/launch/test_state_machine_relay.launch

@@ -6,6 +6,6 @@
 
   <!-- The argument 6 is to notify the code that it is in testing mode for the robot Relay -->
   <group ns = "test_robot_2">
-    <node pkg="mdis_state_machine" type="team_scheduler" name="team_scheduler" args="6 dummy_parent dummy_child" output="screen"/>
+    <node pkg="mdis_state_machine" type="team_scheduler" name="team_scheduler" args="6 192.168.0.2:8765 192.168.0.3:8765" output="screen"/>
   </group>
 </launch>

mdis_state_machine/package.xml

@@ -14,6 +14,7 @@
   <exec_depend>message_runtime</exec_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
+  <depend>coms</depend>
   <depend>octomap</depend>
   <depend>octomap_msgs</depend>
 

mdis_state_machine/src/mdis_state_machine/utils.py

@@ -1,7 +1,7 @@
 import rospy
 from subprocess import Popen, DEVNULL
 from mdis_state_machine.msg import RobotsState
-from mdis_state_machine.msg import Connection
+from coms.msg import nearby
 from std_msgs.msg import String
 from typing import List
 from roslaunch.parent import ROSLaunchParent
@@ -21,7 +21,7 @@
 max_time_to_wait_to_change_state = 6
 message_wait_timeout = 30
 robot_state_topic = "/robots_state"
-connection_check_topic = "/connection_check"
+nearby_check_topic = "/nearby"
 
 def gen_topic_name(lst: List[str]) -> str:
 	result = ""
@@ -64,16 +64,15 @@ def verifyConnStateChange(init_state, change_state, robot_name):
 		if msg.robot_state == init_state:
 			init_state_sucs = True
 
-	robot_conn_msg = Connection()
-	robot_conn_msg.connection_between = []
-	robot_conn_msg.connection_between.append(String(data=robot_name))
-	robot_conn_msg.connection_between.append(String(data="dummy_parent"))
+	payload = nearby()
+	payload.remote_addresses = ["192.168.0.2:8765"]
+	payload.local_address = "192.168.0.1:8765"
 
-	connection_topic_name = gen_topic_name([robot_name, connection_check_topic])
-	pub = rospy.Publisher(connection_topic_name, Connection, queue_size=10)
+	nearby_topic_name = gen_topic_name([robot_name, nearby_check_topic])
+	pub = rospy.Publisher(nearby_topic_name, nearby, queue_size=10)
 	time_start = rospy.get_rostime().secs
 	while rospy.get_rostime().secs < time_start + max_time_to_wait_to_change_state:
-		pub.publish(robot_conn_msg)
+		pub.publish(payload)
 		rospy.sleep(0.5)
 		msg = rospy.wait_for_message(state_topic_name, RobotsState, timeout=message_wait_timeout)
 		if msg.robot_name.data == robot_name:

mdis_state_machine/src/robot_state_machine.cpp

@@ -218,12 +218,12 @@ bool GoToMeet::entryPoint()
 
 bool GoToMeet::isDone()
 {
-   if(connected)
-   {
-     if(isConnDirectRelated(conn_robot))
-     {
-       ROS_INFO("Robot is connected to the party of interest");
-       return true;
+   if(connected) {
+     for(auto robot : conn_robots) {
+      if(isConnDirectRelated(robot)) {
+        ROS_INFO("Robot is connected to the party of interest");
+        return true;
+      }
      }
    }
    return false;
@@ -257,18 +257,10 @@ void GoToMeet::exitPoint()
 }
 
 
-void GoToMeet::connCB(const mdis_state_machine::Connection::ConstPtr msg)
+void GoToMeet::connCB(const coms::nearby::ConstPtr msg)
 {
-  connected = false;
-  for(int i = 0; i<msg->connection_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;
-    }
-  }
+  connected = msg->remote_addresses.size() > 0;
+  conn_robots = msg->remote_addresses;
 }
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 ///////////////////////////////////// M E E T   S T A T E   C L A S S ////////////////////////////////////

turtlebot3_simulations/turtlebot3_gazebo/launch/multi_turtlebot3_all.launch

@@ -54,7 +54,7 @@
     <node pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find turtlebot3_gazebo)/rviz/path_following.rviz"/>
   </group>
 
-  <node pkg="mdis_state_machine" type="connection_check" name="connection_check" args="2"/>
+  <!-- <node pkg="mdis_state_machine" type="connection_check" name="connection_check" args="2"/> -->
 
   <node pkg="tf" type="static_transform_publisher" name="world_to_bot_0_tf_broadcaster"  args="0 0 0 0 0 0 /map /tb3_0/map 100"/>
   <node pkg="tf" type="static_transform_publisher" name="world_to_bot_1_tf_broadcaster"  args="0 0 0 0 0 0 /map /tb3_1/map 100"/>