diff --git a/sr_error_reporter/src/UnderactuationErrorReporter.cpp b/sr_error_reporter/src/UnderactuationErrorReporter.cpp index 716c93b5..9762013c 100644 --- a/sr_error_reporter/src/UnderactuationErrorReporter.cpp +++ b/sr_error_reporter/src/UnderactuationErrorReporter.cpp @@ -1,6 +1,6 @@ /* * Software License Agreement (BSD License) -* Copyright © 2021-2023 belongs to Shadow Robot Company Ltd. +* Copyright © 2021-2024 belongs to Shadow Robot Company Ltd. * All rights reserved. * * Redistribution and use in source and binary forms, with or without modification, @@ -39,11 +39,6 @@ UnderactuationErrorReporter::UnderactuationErrorReporter(ros::NodeHandle& node_handle) : node_handle_(node_handle) { - trajectory_subscriber_left_ = node_handle.subscribe("/lh_trajectory_controller/state", 1, - &UnderactuationErrorReporter::trajectory_callback_left, this, ros::TransportHints().tcpNoDelay()); - trajectory_subscriber_right_ = node_handle.subscribe("/rh_trajectory_controller/state", 1, - &UnderactuationErrorReporter::trajectory_callback_right, this, ros::TransportHints().tcpNoDelay()); - if (!wait_for_param(node_handle, "robot_description")) { ROS_ERROR("UnderactuationErrorReporter did't find robot_description on parameter server"); @@ -58,6 +53,11 @@ UnderactuationErrorReporter::UnderactuationErrorReporter(ros::NodeHandle& node_h { ROS_ERROR("UnderactuationErrorReporter did't find robot_description_semantic on parameter server"); } + + trajectory_subscriber_left_ = node_handle.subscribe("/lh_trajectory_controller/state", 1, + &UnderactuationErrorReporter::trajectory_callback_left, this, ros::TransportHints().tcpNoDelay()); + trajectory_subscriber_right_ = node_handle.subscribe("/rh_trajectory_controller/state", 1, + &UnderactuationErrorReporter::trajectory_callback_right, this, ros::TransportHints().tcpNoDelay()); } ros::Publisher UnderactuationErrorReporter::get_or_create_publisher( diff --git a/sr_multi_moveit/sr_multi_moveit_test/scripts/test_planners.py b/sr_multi_moveit/sr_multi_moveit_test/scripts/test_planners.py index 109105d8..fbdfeccc 100755 --- a/sr_multi_moveit/sr_multi_moveit_test/scripts/test_planners.py +++ b/sr_multi_moveit/sr_multi_moveit_test/scripts/test_planners.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # Software License Agreement (BSD License) -# Copyright © 2019, 2022-2023 belongs to Shadow Robot Company Ltd. +# Copyright © 2019, 2022-2024 belongs to Shadow Robot Company Ltd. # All rights reserved. # # Redistribution and use in source and binary forms, with or without modification, @@ -267,8 +267,7 @@ def test_waypoints(self): wpose = waypoints[0] waypoints.append(copy.deepcopy(wpose)) - (plan3, fraction) = self.group.compute_cartesian_path( - waypoints, 0.01, 0.0) + (plan3, fraction) = self.group.compute_cartesian_path(waypoints, 0.01) if not self._check_plan(plan3) and fraction > 0.8: self.fail_list.append("Failed: test_waypoints, " + self.planner) else: diff --git a/sr_robot_commander/src/sr_robot_commander/follow_warehouse_trajectory.py b/sr_robot_commander/src/sr_robot_commander/follow_warehouse_trajectory.py index 14bcbc16..c7710f0c 100755 --- a/sr_robot_commander/src/sr_robot_commander/follow_warehouse_trajectory.py +++ b/sr_robot_commander/src/sr_robot_commander/follow_warehouse_trajectory.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # Software License Agreement (BSD License) -# Copyright © 2019, 2022-2023 belongs to Shadow Robot Company Ltd. +# Copyright © 2019, 2022-2024 belongs to Shadow Robot Company Ltd. # All rights reserved. # # Redistribution and use in source and binary forms, with or without modification, @@ -52,7 +52,6 @@ def __init__(self): rospy.sleep(2) group_id = rospy.get_param("~arm_group_name") self.eef_step = rospy.get_param("~eef_step", 0.01) - self.jump_threshold = rospy.get_param("~jump_threshold", 1000) self.group = MoveGroupCommander(group_id) self._robot_name = self.robot._r.get_robot_name() @@ -151,8 +150,7 @@ def plan_from_list(self, waypoint_names): # +1 because current position is used as first waypiont. rospy.logerr("Not all waypoints existed, not executing.") return False - (plan, fraction) = self.group.compute_cartesian_path( - waypoints, self.eef_step, self.jump_threshold) + (plan, fraction) = self.group.compute_cartesian_path(waypoints, self.eef_step) if fraction < self._min_wp_fraction: rospy.logerr("Only managed to generate trajectory through" + diff --git a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py index 651c1f4e..ec21952a 100644 --- a/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py +++ b/sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # Software License Agreement (BSD License) -# Copyright © 2015, 2022-2023 belongs to Shadow Robot Company Ltd. +# Copyright © 2015, 2022-2024 belongs to Shadow Robot Company Ltd. # All rights reserved. # # Redistribution and use in source and binary forms, with or without modification, @@ -808,15 +808,13 @@ def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True): rospy.loginfo("Trajectory not completed") def plan_to_waypoints_target(self, waypoints, reference_frame=None, - eef_step=0.005, jump_threshold=0.0, custom_start_state=None): + eef_step=0.005, custom_start_state=None): """ Specify a set of waypoints for the end-effector and plans. This is a blocking method. @param reference_frame - the reference frame in which the waypoints are given. @param waypoints - an array of poses of end-effector. @param eef_step - configurations are computed for every eef_step meters. - @param jump_threshold - maximum distance in configuration space between consecutive points in the - resulting path. @param custom_start_state - specify a start state different than the current state. @return - motion plan (RobotTrajectory msg) that contains the trajectory to the set wayapoints targets. """ @@ -827,7 +825,7 @@ def plan_to_waypoints_target(self, waypoints, reference_frame=None, old_frame = self._move_group_commander.get_pose_reference_frame() if reference_frame is not None: self.set_pose_reference_frame(reference_frame) - self.__plan, fraction = self._move_group_commander.compute_cartesian_path(waypoints, eef_step, jump_threshold) + self.__plan, fraction = self._move_group_commander.compute_cartesian_path(waypoints, eef_step) self.set_pose_reference_frame(old_frame) return self.__plan, fraction diff --git a/sr_robot_launch/launch/sr_ur_arm_hand.launch b/sr_robot_launch/launch/sr_ur_arm_hand.launch index 819d3b5c..e64f729e 100644 --- a/sr_robot_launch/launch/sr_ur_arm_hand.launch +++ b/sr_robot_launch/launch/sr_ur_arm_hand.launch @@ -1,6 +1,6 @@ - + diff --git a/sr_robot_launch/launch/sr_ur_arms_hands.launch b/sr_robot_launch/launch/sr_ur_arms_hands.launch index 510cf6e9..6932d0eb 100644 --- a/sr_robot_launch/launch/sr_ur_arms_hands.launch +++ b/sr_robot_launch/launch/sr_ur_arms_hands.launch @@ -1,6 +1,6 @@ - + diff --git a/sr_robot_launch/launch/srhand.launch b/sr_robot_launch/launch/srhand.launch index 83386eac..6f67aa41 100644 --- a/sr_robot_launch/launch/srhand.launch +++ b/sr_robot_launch/launch/srhand.launch @@ -1,6 +1,6 @@ - - + + - + @@ -135,5 +135,5 @@ - +