Merge branch 'develop' into 'master'

fix 4.0.1

See merge request niryo/niryo-one-s/ned_ros_stack!302

niryo_robot_poses_handlers/scripts/poses_handlers_node.py

@@ -65,7 +65,7 @@ def __init__(self):
         rospy.Service('~get_target_pose', GetTargetPose, self.__callback_target_pose)
 
         # Transform Handlers
-        self.__transform_handler = PosesTransformHandler(self.__grip_manager)
+        self.__transform_handler = PosesTransformHandler(self.__grip_manager, self)
 
         rospy.logdebug("Poses Handlers - Transform Handler created")
 
@@ -417,6 +417,7 @@ def get_target_pose(self, workspace, height_offset, x_rel, y_rel, yaw_rel):
         current_ws = self.__ws_manager.read(workspace)
         self.__transform_handler.set_relative_pose_object(current_ws, x_rel, y_rel, yaw_rel,
                                                           yaw_center=current_ws.yaw_center)
+
         if self.__tcp_enabled:
             base_link_to_tool_target = self.__transform_handler.get_object_transform(z_off=height_offset)
         else:

niryo_robot_poses_handlers/scripts/transform_handler.py

@@ -1,16 +1,17 @@
 #!/usr/bin/env python
 
 # Libs
+import copy
+
 import rospy
 import tf2_ros
-
 import niryo_robot_poses_handlers.transform_functions as transformations
-
+from tf2_ros import StaticTransformBroadcaster
 import threading
 import numpy as np
 
 # Messages
-from geometry_msgs.msg import TransformStamped
+from geometry_msgs.msg import TransformStamped, Vector3, Quaternion
 from visualization_msgs.msg import Marker
 
 
@@ -19,12 +20,12 @@ class PosesTransformHandler:
     This class uses a tfBuffer to handle transforms related to the vision kit.
     """
 
-    def __init__(self, grip_manager):
+    def __init__(self, grip_manager, pose_handler_node):
         self.__tf_buffer = tf2_ros.Buffer()
         self.__debug_stop_event = threading.Event()
         self.__debug_thread = None
         self.__debug_current_ws = None  # only for debugging purposes
-
+        self.__pose_handler_node = pose_handler_node
         self.__grip_manager = grip_manager
 
     def __del__(self):
@@ -40,41 +41,35 @@ def set_relative_pose_object(self, workspace, x_rel, y_rel, yaw_rel, yaw_center=
         :param yaw_rel: object base rotation on z relative to workspace
         :param yaw_center: Avoid over rotation
         """
-        position = np.dot(workspace.position_matrix, np.array([x_rel, y_rel, 1]))
-        camera_rotation = transformations.euler_matrix(0, 0, yaw_rel)
-        # Here we correct the object orientation to be similar to base_link if
-        # the object in on the ground. Not neccessarily needed to be honest...
-        convention_rotation = np.array([[0, -1, 0, 0],
-                                        [-1, 0, 0, 0],
-                                        [0, 0, -1, 0],
-                                        [0, 0, 0, 1]])
-
-        object_rotation = transformations.concatenate_matrices(
-            workspace.rotation_matrix, camera_rotation, convention_rotation)
-        roll, pitch, yaw = transformations.euler_from_matrix(object_rotation)
+        tmp_buffer = tf2_ros.Buffer()
+        delta_x = transformations.euclidian_dist(workspace.points[1], workspace.points[0]) * x_rel
+        delta_y = transformations.euclidian_dist(workspace.points[3], workspace.points[0]) * y_rel
+
+        t = TransformStamped()
+        t.transform.translation = Vector3(delta_y, delta_x, 0)
+        t.transform.rotation = Quaternion(*transformations.quaternion_from_euler(0, 0, yaw_rel))
+        t.header.frame_id = str(workspace.name)
+        t.child_frame_id = "object_base"
+        tmp_buffer.set_transform(t, "default_authority")
+        t.child_frame_id = "object_base_bis"
+
+        transform_frame = self.__pose_handler_node.dynamic_frame_manager.dict_dynamic_frame[workspace.name]["transform"]
+        transform = copy.deepcopy(transform_frame)
+        transform.header.frame_id = "base_link"
+        transform.header.stamp = rospy.Time(0)
+        tmp_buffer.set_transform(transform, "default_authority")
 
         # Correcting yaw to avoid out of reach targets
+        t = tmp_buffer.lookup_transform("base_link", "object_base", rospy.Time(0))
+        roll, pitch, yaw = transformations.euler_from_quaternion([t.transform.rotation.x, t.transform.rotation.y,
+                                                                  t.transform.rotation.z, t.transform.rotation.w])
         if yaw_center is not None:
             if yaw < yaw_center - np.pi / 2:
                 yaw += np.pi
             elif yaw > yaw_center + np.pi / 2:
                 yaw -= np.pi
 
-        q = transformations.quaternion_from_euler(roll, pitch, yaw)
-
-        t = TransformStamped()
-        t.transform.translation.x = position[0]
-        t.transform.translation.y = position[1]
-        t.transform.translation.z = position[2]
-
-        t.transform.rotation.x = q[0]
-        t.transform.rotation.y = q[1]
-        t.transform.rotation.z = q[2]
-        t.transform.rotation.w = q[3]
-
-        t.header.frame_id = "base_link"
-        t.child_frame_id = "object_base"
-
+        t.transform.rotation = Quaternion(*transformations.quaternion_from_euler(roll, pitch, yaw))
         self.__tf_buffer.set_transform(t, "default_authority")
 
     def set_grip(self, grip):

niryo_robot_poses_handlers/src/niryo_robot_poses_handlers/dynamic_frame_manager.py

@@ -4,7 +4,7 @@
 from niryo_robot_poses_handlers.file_manager import FileManager, NiryoRobotFileException
 from niryo_robot_poses_handlers.transform_functions import euler_from_matrix, quaternion_from_euler
 
-from geometry_msgs.msg import Point, TransformStamped, Pose, Quaternion
+from geometry_msgs.msg import Point, Vector3, TransformStamped, Pose, Quaternion
 
 import tf2_ros
 import numpy as np
@@ -111,7 +111,7 @@ def create(self, frame_name, points, description="", belong_to_workspace=False):
         static_transform_stamped.header.frame_id = "world"
         static_transform_stamped.child_frame_id = frame_name
         # Position
-        static_transform_stamped.transform.translation = Point(*[point_o.x, point_o.y, point_o.z])
+        static_transform_stamped.transform.translation = Vector3(*[point_o.x, point_o.y, point_o.z])
         # Orientation
         static_transform_stamped.transform.rotation = Quaternion(*q)
 
@@ -145,7 +145,7 @@ def edit_frame(self, frame_name, new_frame_name, description=""):
         static_transform_stamped.header.frame_id = "world"
         static_transform_stamped.child_frame_id = new_frame_name
         # Position
-        static_transform_stamped.transform.translation = Point(*[point_o.x, point_o.y, point_o.z])
+        static_transform_stamped.transform.translation = Vector3(*[point_o.x, point_o.y, point_o.z])
         # Orientation
         static_transform_stamped.transform.rotation = Quaternion(*q)
 
@@ -196,9 +196,9 @@ def restore_publisher(self):
             # Create frame
             static_transform_stamped = TransformStamped()
             static_transform_stamped.header.frame_id = "world"
-            static_transform_stamped.child_frame_id = frame.name
+            static_transform_stamped.child_frame_id = str(frame.name)
             # Position
-            static_transform_stamped.transform.translation = Point(*[point_o.x, point_o.y, point_o.z])
+            static_transform_stamped.transform.translation = Vector3(*[point_o.x, point_o.y, point_o.z])
             # Orientation
             static_transform_stamped.transform.rotation = Quaternion(*q)
 

niryo_robot_poses_handlers/src/niryo_robot_poses_handlers/transform_functions.py

@@ -45,8 +45,8 @@ def euler_matrix(ai, aj, ak, axes='sxyz'):
         firstaxis, parity, repetition, frame = axes
 
     i = firstaxis
-    j = _NEXT_AXIS[i+parity]
-    k = _NEXT_AXIS[i-parity+1]
+    j = _NEXT_AXIS[i + parity]
+    k = _NEXT_AXIS[i - parity + 1]
 
     if frame:
         ai, ak = ak, ai
@@ -55,30 +55,30 @@ def euler_matrix(ai, aj, ak, axes='sxyz'):
 
     si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak)
     ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak)
-    cc, cs = ci*ck, ci*sk
-    sc, ss = si*ck, si*sk
+    cc, cs = ci * ck, ci * sk
+    sc, ss = si * ck, si * sk
 
     M = np.identity(4)
     if repetition:
         M[i, i] = cj
-        M[i, j] = sj*si
-        M[i, k] = sj*ci
-        M[j, i] = sj*sk
-        M[j, j] = -cj*ss+cc
-        M[j, k] = -cj*cs-sc
-        M[k, i] = -sj*ck
-        M[k, j] = cj*sc+cs
-        M[k, k] = cj*cc-ss
+        M[i, j] = sj * si
+        M[i, k] = sj * ci
+        M[j, i] = sj * sk
+        M[j, j] = -cj * ss + cc
+        M[j, k] = -cj * cs - sc
+        M[k, i] = -sj * ck
+        M[k, j] = cj * sc + cs
+        M[k, k] = cj * cc - ss
     else:
-        M[i, i] = cj*ck
-        M[i, j] = sj*sc-cs
-        M[i, k] = sj*cc+ss
-        M[j, i] = cj*sk
-        M[j, j] = sj*ss+cc
-        M[j, k] = sj*cs-sc
+        M[i, i] = cj * ck
+        M[i, j] = sj * sc - cs
+        M[i, k] = sj * cc + ss
+        M[j, i] = cj * sk
+        M[j, j] = sj * ss + cc
+        M[j, k] = sj * cs - sc
         M[k, i] = -sj
-        M[k, j] = cj*si
-        M[k, k] = cj*ci
+        M[k, j] = cj * si
+        M[k, k] = cj * ci
     return M
 
 
@@ -134,9 +134,9 @@ def quaternion_matrix(quaternion):
     q *= math.sqrt(2.0 / nq)
     q = np.outer(q, q)
     return np.array((
-        (1.0-q[1, 1]-q[2, 2], 0.0+q[0, 1]-q[2, 3], 0.0+q[0, 2]+q[1, 3], 0.0),
-        (0.0+q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], 0.0+q[1, 2]-q[0, 3], 0.0),
-        (0.0+q[0, 2]-q[1, 3], 0.0+q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0),
+        (1.0 - q[1, 1] - q[2, 2], 0.0 + q[0, 1] - q[2, 3], 0.0 + q[0, 2] + q[1, 3], 0.0),
+        (0.0 + q[0, 1] + q[2, 3], 1.0 - q[0, 0] - q[2, 2], 0.0 + q[1, 2] - q[0, 3], 0.0),
+        (0.0 + q[0, 2] - q[1, 3], 0.0 + q[1, 2] + q[0, 3], 1.0 - q[0, 0] - q[1, 1], 0.0),
         (0.0, 0.0, 0.0, 1.0)
     ), dtype=np.float64)
 
@@ -186,3 +186,7 @@ def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
         quaternion[j] *= -1
 
     return quaternion
+
+
+def euclidian_dist(point_a, point_b):
+    return np.linalg.norm(np.array(point_a) - np.array(point_b))

niryo_robot_python_ros_wrapper/examples/test_production_ned.py

@@ -582,13 +582,14 @@ def end_test(self, report):
             self.say("Fin du test, validez la position 0")
             report.execute(self.wait_save_button_press, "Wait save button press to validate")
 
+        self.__robot.led_ring.solid(BLUE)
         self.__robot.move_to_sleep_pose()
         self.__robot.set_arm_max_velocity(100)
         self.__robot.set_arm_max_acceleration(100)
 
     def wait_custom_button_press(self, timeout=20):
         if not USE_BUTTON:
-            raw_input('Enter to continue')
+            # raw_input('Enter to continue')
             return 1, "Press custom button step skipped"
 
         action = self.__robot.custom_button.wait_for_any_action(timeout=timeout)
@@ -601,7 +602,7 @@ def wait_custom_button_press(self, timeout=20):
     @staticmethod
     def wait_save_button_press():
         if not USE_BUTTON:
-            raw_input('Enter to continue')
+            # raw_input('Enter to continue')
             return 1, "Press save button step skipped"
 
         try:
@@ -634,18 +635,23 @@ def move_and_compare_without_moveit(self, target, precision_decimal=1, duration=
 if __name__ == '__main__':
     rospy.init_node('niryo_test_production_ros_wrapper')
     robot = NiryoRosWrapper()
-    # robot.system_api_client.set_ethernet_auto()
+
     print("----- START -----")
     test = TestProduction()
     test.run()
     print("----- END -----")
     test.print_report()
-    test.send_report()
-    # robot.system_api_client.set_ethernet_static()
 
-    if test.get_report()['success']:
+    try:
+        robot.system_api_client.set_ethernet_auto()
+        rospy.sleep(10)
+
+        test.send_report()
+
         set_setting = rospy.ServiceProxy('/niryo_robot_database/settings/set', SetSettings)
         # set_setting('sharing_allowed', 'False', 'bool')
         set_setting('test_report_done', 'True', 'bool')
-    else:
-        raise TestFailure()
+    except Exception as _e:
+        TestFailure()
+
+    robot.system_api_client.set_ethernet_static()

niryo_robot_reports/scripts/auto_diagnosis.py

@@ -49,7 +49,7 @@ def __str__(self):
 
     def append(self, message):
         new_line = "[{}] - {} - {}.".format(self._header, datetime.now(), message)
-        print(new_line)
+        rospy.loginfo('[Test Report]' + new_line)
         self._report += new_line + "\\n"
 
     def execute(self, function, prefix="", args=None):
@@ -156,9 +156,10 @@ def __init__(self):
         self.__robot.set_arm_max_acceleration(ACCELERATION)
 
         if self.__robot_version in ['ned2']:
+            rospy.sleep(2)
             self.__robot.led_ring.rainbow()
-            self.__robot.sound.set_volume(100)
-            self.__robot.sound.say('Initialization of the auto-diagnosis')
+            # self.__robot.sound.set_volume(100)
+            # self.__robot.sound.say('Initialization of the auto-diagnosis')
             rospy.sleep(3)
         else:
             self.led_stop()

niryo_robot_system_api_client/CMakeLists.txt

@@ -9,16 +9,27 @@ find_package(catkin REQUIRED COMPONENTS
 # enable python module support
 catkin_python_setup()
 
+file(
+  GLOB msg_files
+  RELATIVE  ${CMAKE_CURRENT_SOURCE_DIR}/msg
+  msg/*.msg
+)
+
 add_message_files(
   FILES
-  WifiStatus.msg
+  ${msg_files}
+)
+
+file(
+  GLOB srv_files
+  RELATIVE  ${CMAKE_CURRENT_SOURCE_DIR}/srv
+  srv/*.srv
 )
 
 add_service_files(
   DIRECTORY srv
   FILES
-  ManageEthernet.srv
-  ManageWifi.srv
+  ${srv_files}
 )
 
 generate_messages(