Merge branch 'master' of https://github.com/SkoltechRobotics/ros-euro…

…bot-2018

eurobot/launch/multimaster.launch

@@ -1,7 +1,9 @@
 <?xml version="1.0"?>
 <launch>
 
-  <node pkg="master_discovery_fkie" type="master_discovery" name="master_discovery" />
-  <node pkg="master_sync_fkie" type="master_sync" name="master_sync" />
+    <node pkg="master_discovery_fkie" type="master_discovery" name="master_discovery" respawn="true">
+        <rosparam file="$(find eurobot)/launch/multimaster_config.yaml" command="load" />
+    </node>    
+    <node pkg="master_sync_fkie" type="master_sync" name="master_sync" respawn="true"/>
 
 </launch>

eurobot/launch/multimaster_config.yaml

@@ -0,0 +1,3 @@
+interface: "192.168.88.244"
+
+

eurobot/launch/secondary_multimaster.launch

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<launch>
+
+    <node pkg="master_discovery_fkie" type="master_discovery" name="master_discovery" respawn="true">
+        <rosparam file="$(find eurobot)/launch/secondary_multimaster_config.yaml" command="load" />
+    </node>    
+    <node pkg="master_sync_fkie" type="master_sync" name="master_sync" respawn="true"/>
+
+</launch>

eurobot/launch/secondary_multimaster_config.yaml

@@ -0,0 +1,3 @@
+interface: "192.168.88.242"
+
+

eurobot/launch/secondary_robot.launch

@@ -8,7 +8,7 @@
   <include file="$(find eurobot)/launch/params.launch"/>
 
   <!-- MULTYMASTER -->
-  <include file="$(find eurobot)/launch/multimaster.launch"/>
+  <include file="$(find eurobot)/launch/secondary_multimaster.launch"/>
 
   <include file="$(find eurobot_navigation)/launch/navigation_secondary.launch"/>
 

eurobot/main_robot_multimaster

@@ -3,7 +3,7 @@ date1=`python2 /home/odroid/catkin_ws/src/ros-eurobot-2018/scripts/make_older.py
 date +%Y%m%d%T -s $date1
 source /opt/ros/kinetic/setup.bash
 source ~/catkin_ws/devel/setup.bash
-export ROS_IP=192.168.88.244
+#export ROS_IP=192.168.88.244
 export DISPLAY=:0.0
 sleep 1s
 while true; do

eurobot/scripts/stm_node/STMprotocol.py

@@ -43,7 +43,8 @@ def __init__(self, serial_port, baudrate=64000):
             0xa3: "=",
             0xe0: "=B",
             256: "=H",
-            0xa4: ""
+            0xa4: "",
+            0xb7: "=B"
         }
 
         self.unpack_format = {
@@ -80,7 +81,8 @@ def __init__(self, serial_port, baudrate=64000):
             # 0xd0: "=BBBBBB",
             0xa3: "=B",
             0xe0: "=BB",
-            0xa4: "=BB"
+            0xa4: "=BB",
+            0xb7: "=BB"
         }
 
     def pure_send_command(self, cmd, args):

eurobot/scripts/stm_node/stm_node.py

@@ -18,7 +18,7 @@
 GET_STARTUP_STATUS = 0xa3
 GET_SEC_ROBOT_MANIPULATOR_STATUS = 0xc0
 
-MANIPULATOR_JOBS = [0xb0, 0xb1, 0xb2, 0xb3, 0xb4, 0xb5, 0xc1, 0xc2, 0xc3]
+MANIPULATOR_JOBS = [0xb0, 0xb1, 0xb2, 0xb3, 0xb4, 0xb5, 0xb7, 0xc1, 0xc2, 0xc3]
 IMMEDIATE_FINISHED = [0xc4, 0xb6, 0xe0]
 ODOMETRY_MOVEMENT = [0xa2]
 DEBUG_COMMANDS = [0x0c]

eurobot/secondary_robot_multimaster

@@ -2,7 +2,7 @@
 
 source /opt/ros/kinetic/setup.bash
 source ~/catkin_ws/devel/setup.bash
-export ROS_IP=192.168.88.242
+#export ROS_IP=192.168.88.242
 sleep 1s
 while true; do
     roslaunch eurobot secondary_robot.launch color:=$1

eurobot_decision_maker/scripts/bt_builder.py

@@ -258,8 +258,8 @@ def add_switch_secondary(self, parent_name):
             self.add_command_action(main_seq_name, 162, 0.2, 0, 0, 0.57, 0, 0)
             self.add_command_action(main_seq_name, 224, 1) # collision avoidance
             self.add_command_action(main_seq_name, 182, 0) # manipulator
-        else: # TODO: change coords
-            self.add_action_node(main_seq_name, "move", self.move_publisher_name, self.move_response, "move", 1.947, 0.25,
+        else:
+            self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 1.947, 0.25,
                                  0.79)
             self.add_command_action(main_seq_name, 162, 0, 0, 0.75, 0, 0, 6)
             self.add_command_action(main_seq_name, 182, 1) # manipulator
@@ -284,7 +284,8 @@ def add_start_switch_main_new(self, parent_name):
         else:
             self.add_command_action(main_seq_name, 224, 0)
             self.add_simple_move(main_seq_name, "move_odometry", 3 - 0.4, 0.2, np.pi)
-            self.add_simple_move(main_seq_name, "move", 3 - x_dist, 0.2, np.pi)
+            # self.add_simple_move(main_seq_name, "move", 3 - x_dist, 0.2, np.pi)
+            self.add_simple_move(main_seq_name, "move_odometry", 3 - x_dist, 0.2, np.pi)
             self.add_simple_move(main_seq_name, "move_odometry", 3 - x_dist, 0.0, np.pi)
             self.add_simple_move(main_seq_name, "move_odometry", 3 - x_dist, 0.25, np.pi)
             self.add_command_action(main_seq_name, 224, 1)
@@ -414,24 +415,27 @@ def add_cubes_secondary(self, parent_name):
             self.add_action_node(main_seq_name, "rotate_odometry", self.move_publisher_name, self.move_response, "rotate_odometry", 0, 2)
             self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 1.1, 1.2, 0)
             self.add_command_action(main_seq_name, 194, 2) # manipulators
-            self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 1.1, 1.45, 0)
+            self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 1.1, 1.4, 0)
+            self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_odometry", 1.1, 1.47, 0)
             self.add_command_action(main_seq_name, 194, 1) # manipulators
 
     def add_wastewater_tower_after_cubes(self, parent_name):
-        main_seq_name = self.construct_string("wastewater_after_cubes", self.get_next_id())
+        to = "left" if self.side == "orange" else "right"
+        to_sorter = 0 if to == "left" else 1
+        main_seq_name = self.construct_string("wastewater_tower_after_cubes", self.get_next_id())
         self.add_sequence_node(parent_name, main_seq_name)
 
         if self.side == "orange":
             self.add_action_node(main_seq_name, "rotate_odometry", self.move_publisher_name, self.move_response, "rotate_odometry", 2 * np.pi - 0.53, 2)
-            self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 1, 1.2, -0.53)
+            self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 1, 1.2, 2 * np.pi - 0.53)
             self.add_action_node(main_seq_name, "rotate_odometry", self.move_publisher_name, self.move_response, "rotate_odometry", 3.14, 2)
             self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 2.25, 1.2, 3.14)
             self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 2.25, 1.8, 3.14, 0, 1, 1)
         else:
-            self.add_action_node(main_seq_name, "rotate_odometry", self.move_publisher_name, self.move_response, "rotate_odometry", 4.71, 2)
-            self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 0.7, 1.45, 0) #
-            self.add_action_node(main_seq_name, "rotate_odometry", self.move_publisher_name, self.move_response, "rotate_odometry", 3.14, 2)
-            self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 0.7, 1.8, 3.14, 1, 1, 0)
+            self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 0.95, 1.55, 4.71)
+            self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 0.7, 1.55, 4.71)
+            self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 0.7, 1.65, 4.71)
+            self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 0.7, 1.8, 2.9, 1, 1, 0)
 
         self.add_shooting_motor_action(main_seq_name, to, "slow")
         self.add_sleep_time(main_seq_name, .5)
@@ -441,11 +445,8 @@ def add_wastewater_tower_after_cubes(self, parent_name):
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 2.390, 1.844, 3.14, 0.05, .2)
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 2.390, 1.844, 3.14, 0.05, .2)
         else:
-            self.add_sleep_time(main_seq_name, 0.3)
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 3 - 2.390, 1.844, 3.14, 0.1, .2)
-            self.add_sleep_time(main_seq_name, 0.3)
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 3 - 2.390, 1.844, 3.14, 0.05, .2)
-            self.add_sleep_time(main_seq_name, 0.3)
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 3 - 2.390, 1.844, 3.14, 0.05, .2)
 
         self.add_sleep_time(main_seq_name, 3)
@@ -568,6 +569,12 @@ def add_sequence_node(self, parent_name, new_name):
             rospy.loginfo(ss)
         self.bt.add_node_by_string(ss)
 
+    def add_parallel_node(self, parent_name, new_name):
+        ss = self.construct_string(parent_name, "parallel", new_name, sep=' ')
+        if self.loginfo:
+            rospy.loginfo(ss)
+        self.bt.add_node_by_string(ss)
+
     def add_rf_move(self, parent_name, heap_status, colors=[], mans=[]):
         # add heap_status
         if self.loginfo:
@@ -730,6 +737,7 @@ def add_new_heap_pick_no_rf(self, parent_name, heap_num, heap_strat, next_heap_n
         self.add_command_action(main_seq_name, 224, 0)
         self.add_simple_move(main_seq_name, "face_heap", heap_num)
         self.add_simple_move(main_seq_name, "move_heap", heap_num)
+        # self.add_simple_move(main_seq_name, "move_odometry")
 
 
         self.colors_left = {0, 1, 2, 3, 4}
@@ -747,6 +755,13 @@ def add_new_heap_pick_no_rf(self, parent_name, heap_num, heap_strat, next_heap_n
                 rospy.loginfo("--------COLORS MANS  " + str(colors) +' '+ str(mans))
             a += da
             self.add_heap_rotation_no_rf(main_seq_name, heap_num, a)
+            if i == 0:
+                form_par_name = self.construct_string("form_cubes", heap_num)
+                self.add_parallel_node(main_seq_name, form_par_name)
+                for i in range(3):
+                    self.add_command_action(form_par_name, 0xb7, i)
+
+
             if dx ** 2 + dy ** 2 > 0:
                 if self.loginfo:
                     rospy.loginfo("SHIFTS " + str(self.shifts.index((dx, dy))))
@@ -766,7 +781,7 @@ def add_new_heap_pick_no_rf(self, parent_name, heap_num, heap_strat, next_heap_n
                 rospy.loginfo(a)
             if self.loginfo:
                 rospy.loginfo(mans)
-            self.add_sleep_time(main_seq_name, 5)
+            #self.add_sleep_time(main_seq_name, 5)
 
             if 1 in mans:
                 if len(mans) != 1:
@@ -786,7 +801,7 @@ def add_new_heap_pick_no_rf(self, parent_name, heap_num, heap_strat, next_heap_n
                 self.add_cubes_pick(main_seq_name, heap_num, mans, colors, new=True, doors=False)
 
     def add_simple_move(self, parent_name, move_type,  *coords, **kvargs):
-        delay = 0.6
+        delay = 0.3
         repeat = 1
         if 'repeat' in kvargs:
             repeat = kvargs['repeat']
@@ -1163,10 +1178,8 @@ def add_wastewater_tower(self, parent_name, delay=1):
             self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 2.25, 1.2, 3.14)
             self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 2.25, 1.8, 3.14, 0, 1, 1)
         else:
-            self.add_sleep_time(main_seq_name, 0.5)
             self.add_action_node(main_seq_name, "rotate_odometry", self.move_publisher_name, self.move_response, "rotate_odometry", 0.53, 2)
             self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 2, 1.2, 0.53)
-            self.add_sleep_time(main_seq_name, 0.5)
             self.add_action_node(main_seq_name, "rotate_odometry", self.move_publisher_name, self.move_response, "rotate_odometry", 3.14, 2)
             self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 0.7, 1.2, 3.14)
             self.add_action_node(main_seq_name, "move_fast", self.move_publisher_name, self.move_response, "move_fast", 0.7, 1.8, 3.14, 1, 1, 0)
@@ -1179,11 +1192,8 @@ def add_wastewater_tower(self, parent_name, delay=1):
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 2.390, 1.844, 3.14, 0.05, .2)
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 2.390, 1.844, 3.14, 0.05, .2)
         else:
-            self.add_sleep_time(main_seq_name, 0.3)
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 3 - 2.390, 1.844, 3.14, 0.1, .2)
-            self.add_sleep_time(main_seq_name, 0.3)
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 3 - 2.390, 1.844, 3.14, 0.05, .2)
-            self.add_sleep_time(main_seq_name, 0.3)
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 3 - 2.390, 1.844, 3.14, 0.05, .2)
 
         self.add_sleep_time(main_seq_name, 3)
@@ -1246,15 +1256,19 @@ def add_cleanwater_tower(self, parent_name, to="left", with_4_balls=False, only_
 
         self.add_command_action(main_seq_name, 224, 0) # collision avoidance
         self.add_shoot_sort_action(main_seq_name, "release " + to) 
-        self.add_shooting_motor_action(main_seq_name, to, "on")
 
         if self.side == "orange":
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", .200, .700, 4.71, .35, 1.0)
+        else:
+            self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 3 - .200, .700, 1.57, .35, 1.0)
+
+        self.add_shooting_motor_action(main_seq_name, to, "on")
+
+        if self.side == "orange":
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", .156, .840, 4.71, 0.1, .2)
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", .156, .840, 4.71, 0.05, .2)
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", .156, .840, 4.71, 0.05, .2)
         else:
-            self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 3 - .200, .700, 1.57, .35, 1.0)
             self.add_sleep_time(main_seq_name, 0.3)
             self.add_action_node(main_seq_name, "move_odometry", self.move_publisher_name, self.move_response, "move_odometry", 3 - .156, .840, 1.57, 0.1, .2)
             self.add_sleep_time(main_seq_name, 0.3)