diff --git a/niryo_robot_poses_handlers/scripts/poses_handlers_node.py b/niryo_robot_poses_handlers/scripts/poses_handlers_node.py index 7dd963ab4..2aa17fac9 100755 --- a/niryo_robot_poses_handlers/scripts/poses_handlers_node.py +++ b/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: diff --git a/niryo_robot_poses_handlers/scripts/transform_handler.py b/niryo_robot_poses_handlers/scripts/transform_handler.py index 46fca835a..e1d706d33 100644 --- a/niryo_robot_poses_handlers/scripts/transform_handler.py +++ b/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): diff --git a/niryo_robot_poses_handlers/src/niryo_robot_poses_handlers/dynamic_frame_manager.py b/niryo_robot_poses_handlers/src/niryo_robot_poses_handlers/dynamic_frame_manager.py index cd870a301..97cdfa192 100644 --- a/niryo_robot_poses_handlers/src/niryo_robot_poses_handlers/dynamic_frame_manager.py +++ b/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) diff --git a/niryo_robot_poses_handlers/src/niryo_robot_poses_handlers/transform_functions.py b/niryo_robot_poses_handlers/src/niryo_robot_poses_handlers/transform_functions.py index 82c8dd164..ff2acd7f4 100644 --- a/niryo_robot_poses_handlers/src/niryo_robot_poses_handlers/transform_functions.py +++ b/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)) diff --git a/niryo_robot_python_ros_wrapper/examples/test_production_ned.py b/niryo_robot_python_ros_wrapper/examples/test_production_ned.py index 982045e22..a5312644a 100644 --- a/niryo_robot_python_ros_wrapper/examples/test_production_ned.py +++ 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() diff --git a/niryo_robot_reports/scripts/auto_diagnosis.py b/niryo_robot_reports/scripts/auto_diagnosis.py index befb448b2..791d108f4 100755 --- a/niryo_robot_reports/scripts/auto_diagnosis.py +++ b/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() diff --git a/niryo_robot_system_api_client/CMakeLists.txt b/niryo_robot_system_api_client/CMakeLists.txt index d403994ca..68798ab48 100644 --- a/niryo_robot_system_api_client/CMakeLists.txt +++ b/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(