def joint_state_publisher(self): if self.EnableFlag == 1 and self.motion_ctrl.positionUpdatedFlag == '1': try: joint_state_msg = JointState() joint_fb_msg = FollowJointTrajectoryFeedback() time = rospy.Time.now() with self.lock: #Joint states joint_state_msg.header.stamp = time joint_state_msg.name = self.joint_names joint_state_msg.position = self.motion_ctrl.get_joint_positions( ) # self.joint_state_pub.publish(joint_state_msg) #Joint feedback joint_fb_msg.header.stamp = time joint_fb_msg.joint_names = self.joint_names joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions( ) self.joint_feedback_pub.publish(joint_fb_msg) except Exception as e: rospy.loginfo( 'Unexpected exception in joint state publisher: %s', e)
def _state_publisher(self): try: joint_state_msg = JointState() joint_fb_msg = FollowJointTrajectoryFeedback() with self._lock: time = rospy.Time.now() q_actual, q_desired, q_error = self._rsi_connection.get_joint_states( ) #Joint states joint_state_msg.header.stamp = time joint_state_msg.name = self._joint_names joint_state_msg.position = q_actual self._joint_state_pub.publish(joint_state_msg) #Joint feedback joint_fb_msg.header.stamp = time joint_fb_msg.joint_names = self._joint_names joint_fb_msg.actual.positions = q_actual joint_fb_msg.desired.positions = q_desired joint_fb_msg.error.positions = q_error self._joint_feedback_pub.publish(joint_fb_msg) except Exception as e: rospy.logerr('Unexpected exception in joint state publisher: %s', e)
def joint_state_publisher(self): try: joint_state_msg = JointState() joint_fb_msg = FollowJointTrajectoryFeedback() time = rospy.Time.now() with self.lock: #Joint states joint_state_msg.header.stamp = time joint_state_msg.name = self.joint_names joint_state_msg.position = self.motion_ctrl.get_joint_positions() #self.joint_state_pub.publish(joint_state_msg) #Joint feedback joint_fb_msg.header.stamp = time joint_fb_msg.joint_names = self.joint_names joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions() #self.joint_feedback_pub.publish(joint_fb_msg) except Exception as e: rospy.logerr('Unexpected exception in joint state publisher: %s', e)
def feedback_states_publisher(rate): global current_joints global current_velocity publisher = rospy.Publisher('feedback_states', FollowJointTrajectoryFeedback, queue_size = conf_rate) sleeper = rospy.Rate(rate) while not rospy.is_shutdown(): msg = FollowJointTrajectoryFeedback() msg.header.stamp = rospy.Time.now() msg.header.frame_id = conf_base_name msg.joint_names = conf_joint_names msg.actual = JointTrajectoryPoint() # ----------------------------------------------------------- # Critical Section # ----------------------------------------------------------- with lock: msg.actual.positions = current_joints msg.actual.velocities = current_velocity msg.actual.effort = current_effort msg.actual.time_from_start = rospy.get_rostime() - trajectory_start # ----------------------------------------------------------- publisher.publish(msg) sleeper.sleep()
def joint_state_publisher(self): joint_state_msg = JointState() joint_state_msg.header.stamp = rospy.Time.now() joint_state_msg.name = self.ABB_joint_name_list + self.gripper3F_joint_name_list joint_state_msg.position = list(self.ABB_joint_states_list) + list( self.gripper3F_joint_states_list) joint_state_msg.velocity = [] joint_state_msg.effort = [] control_state_msg = FollowJointTrajectoryFeedback() control_state_msg.header.stamp = rospy.Time.now() control_state_msg.joint_names = self.ABB_joint_name_list + self.gripper3F_joint_name_list control_state_msg.actual.positions = list( self.ABB_feedback_states_list) + list( self.gripper3F_feedback_states_list) control_state_msg.desired.positions = list( self.ABB_feedback_states_list) + list( self.gripper3F_feedback_states_list) control_state_msg.error.positions = [ 0 for i in control_state_msg.joint_names ] self.joint_state_pub.publish(joint_state_msg) self.control_state_pub.publish(control_state_msg)
def joint_state_publisher(self): LOG.info( "QWERQWERQWERQWEQWRQWERQWERWQERQWERQWERQWERQWERQWERERQWQREWQWREQWERQWER!" ) print('comecomecomecomecomecomecomecomecomecomecome') joint_state_msg = JointState() joint_state_msg.header.stamp = rospy.Time.now() if self.robot_name == 'NRMK-IndyRP2': joint_state_msg.name = [ 'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6' ] else: joint_state_msg.name = [ 'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5' ] cur_joint_pos = self.indy.get_joint_pos() joint_state_msg.position = utils_transf.degs2rads(cur_joint_pos) joint_state_msg.velocity = [self.vel] joint_state_msg.effort = [] control_state_msg = FollowJointTrajectoryFeedback() control_state_msg.header.stamp = rospy.Time.now() if self.robot_name == 'NRMK-IndyRP2': control_state_msg.joint_names = [ 'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6' ] else: control_state_msg.joint_names = [ 'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5' ] control_state_msg.actual.positions = utils_transf.degs2rads( cur_joint_pos) control_state_msg.desired.positions = utils_transf.degs2rads( cur_joint_pos) control_state_msg.error.positions = [ 0 for i in control_state_msg.joint_names ] self.joint_state_pub.publish(joint_state_msg) self.control_state_pub.publish(control_state_msg)
def read_joint_state_data(self, msg): joint_fb_msg = FollowJointTrajectoryFeedback() joint_fb_msg.header.stamp = rospy.Time.now() joint_fb_msg.joint_names = msg.name joint_fb_msg.actual.positions = msg.position joint_fb_msg.actual.velocities = msg.velocity self.joint_feedback_pub.publish(joint_fb_msg)
def on_goal(self, goal_handle): """Handle a new goal trajectory command.""" # Checks if the joints are just incorrect if set(goal_handle.get_goal().trajectory.joint_names) != set( self.prefixedJointNames): rospy.logerr( "Received a goal with incorrect joint names: (%s)" % ', '.join(goal_handle.get_goal().trajectory.joint_names)) goal_handle.set_rejected() return if not trajectory_is_finite(goal_handle.get_goal().trajectory): rospy.logerr("Received a goal with infinites or NaNs") goal_handle.set_rejected( text="Received a goal with infinites or NaNs") return # Checks that the trajectory has velocities if not has_velocities(goal_handle.get_goal().trajectory): rospy.logerr("Received a goal without velocities") goal_handle.set_rejected(text="Received a goal without velocities") return feedback = FollowJointTrajectoryFeedback() result = FollowJointTrajectoryResult() feedback.joint_names = goal_handle.get_goal().trajectory.joint_names # Orders the joints of the trajectory according to joint_names reorder_trajectory_joints(goal_handle.get_goal().trajectory, self.prefixedJointNames) # Inserts the current setpoint at the head of the trajectory now = self.robot.getTime() print 'trajectory_t0: {}'.format(self.trajectory_t0) point0 = sample_trajectory(self.trajectory, now - self.trajectory_t0) point0.time_from_start = rospy.Duration(0.0) goal_handle.get_goal().trajectory.points.insert(0, point0) feedback.desired.positions = point0 self.trajectory_t0 = now print "i am here" print 'point0: {}'.format(point0.positions) # Replaces the goal self.goal_handle = goal_handle self.trajectory = goal_handle.get_goal().trajectory self.feedback = feedback self.result = result goal_handle.set_accepted()
def tr2_arm_follow_joint_trajectory(goal): global tr2_arm_action_server, tr2_a0_state, tr2_a1_state, tr2_a2_state, tr2_a3_state, tr2_a4_state, tr2_a0_pub, tr2_a1_pub, tr2_a2_pub, tr2_a3_pub, tr2_a4_pub, tr2_mode_servo_pub, tr2_stop_pub tr2_mode_servo_pub.publish(1) tr2_stop_pub.publish(0) time.sleep(0.050) success = True feedback = FollowJointTrajectoryFeedback() result = FollowJointTrajectoryResult() joint_names = goal.trajectory.joint_names feedback.joint_names = joint_names print len(goal.trajectory.points) t_start = datetime.datetime.now() for point in goal.trajectory.points: tr2_a0_pub.publish(point.positions[0]) tr2_a1_pub.publish(point.positions[1]) tr2_a2_pub.publish(point.positions[2]) tr2_a3_pub.publish(point.positions[3]) tr2_a4_pub.publish(point.positions[4]) '''if tr2_arm_action_server.is_preempt_requested(): tr2_arm_action_server.set_preempted() success = False break''' while (datetime.datetime.now() - t_start ).total_seconds() < point.time_from_start.nsecs / 1000000000.0: feedback.desired = point feedback.actual.positions = [ tr2_a0_state, tr2_a1_state, tr2_a2_state, tr2_a3_state, tr2_a4_state ] #feedback.error.positions tr2_arm_action_server.publish_feedback(feedback) tr2_arm_action_server.set_succeeded(result) time.sleep(5) tr2_stop_pub.publish(1)
def follow_trajectory_goal(self, traj_msg: FollowJointTrajectoryGoal, feedback_cb: Optional[Callable] = None, stop_cb: Optional[Callable] = None): result = FollowJointTrajectoryResult() # Interpolate the trajectory to a fine resolution # if you set max_step_size to be large and position tolerance to be small, then things will be jerky if len(traj_msg.trajectory.points) == 0: rospy.loginfo("Ignoring empty trajectory") result.error_code = FollowJointTrajectoryResult.SUCCESSFUL result.error_string = "empty trajectory" return result # construct a list of the tolerances in order of the joint names trajectory_joint_names = traj_msg.trajectory.joint_names tolerance = get_ordered_tolerance_list(trajectory_joint_names, traj_msg.path_tolerance) goal_tolerance = get_ordered_tolerance_list(trajectory_joint_names, traj_msg.goal_tolerance, is_goal=True) interpolated_points = interpolate_joint_trajectory_points(traj_msg.trajectory.points, max_step_size=0.1) if len(interpolated_points) == 0: rospy.loginfo("Trajectory was empty after interpolation") result.error_code = FollowJointTrajectoryResult.SUCCESSFUL result.error_string = "empty trajectory" return result rospy.logdebug(interpolated_points) trajectory_point_idx = 0 t0 = rospy.Time.now() while True: # tiny sleep lets the listeners process messages better, results in smoother following rospy.sleep(1e-6) desired_point = interpolated_points[trajectory_point_idx] command_failed, command_failed_msg = self.robot.send_joint_command(trajectory_joint_names, desired_point) # get feedback actual_point = self.get_actual_trajectory_point(trajectory_joint_names) # let the caller stop if stop_cb is not None: stop, stop_msg = stop_cb(actual_point) else: stop = None stop_msg = "" dt = rospy.Time.now() - t0 error = waypoint_error(actual_point, desired_point) rospy.logdebug_throttle(1, f"{error} {desired_point.time_from_start.to_sec()} {dt.to_sec()}") if desired_point.time_from_start.to_sec() > 0 and dt > desired_point.time_from_start * 5.0: stop = True if trajectory_point_idx == len(interpolated_points) - 1: stop_msg = f"timeout. expected t={desired_point.time_from_start.to_sec()} but t={dt.to_sec()}." \ + f" error to waypoint is {error}, tolerance is {goal_tolerance}" else: stop_msg = f"timeout. expected t={desired_point.time_from_start.to_sec()} but t={dt.to_sec()}." \ + f" error to waypoint is {error}, tolerance is {tolerance}" if command_failed or stop: # command the current configuration self.robot.send_joint_command(trajectory_joint_names, actual_point) rospy.loginfo("Preempt requested, aborting.") if command_failed_msg: rospy.loginfo(command_failed_msg) if stop_msg: rospy.logwarn(stop_msg) result.error_code = -10 result.error_string = stop_msg break # If we're close enough, advance if trajectory_point_idx >= len(interpolated_points) - 1: if waypoint_reached(desired_point, actual_point, goal_tolerance): # we're done! result.error_code = FollowJointTrajectoryResult.SUCCESSFUL break else: if waypoint_reached(desired_point, actual_point, tolerance): trajectory_point_idx += 1 if feedback_cb is not None: feedback = FollowJointTrajectoryFeedback() feedback.header.stamp = rospy.Time.now() feedback.joint_names = trajectory_joint_names feedback.desired = desired_point feedback.actual = actual_point feedback_cb(feedback) return result
def execute_joint_trajectory_cb(self, goal): rospy.loginfo("Trajectory received with %d points", len(goal.trajectory.points)) feedback = FollowJointTrajectoryFeedback() result = FollowJointTrajectoryResult() current_status = self._driver.get_current_gripper_status() # Check trajectory joint names joint_names = goal.trajectory.joint_names if len(joint_names ) != 1 and joint_names[0] != self._driver._joint_name: msg = "Joint trajectory joints do not match gripper joint" rospy.logerr(msg) result.error_code = result.INVALID_JOINTS result.error_string = msg self._joint_trajectory_action_server.set_aborted(result) return # Check trajectory points if len(goal.trajectory.points) == 0: msg = "Ignoring empty trajectory " rospy.logerr(msg) result.error_code = result.INVALID_GOAL result.error_string = msg self._joint_trajectory_action_server.set_aborted(result) return # Process goal trajectory self._processing_goal = True self._is_stalled = False goal_command = CommandRobotiqGripperGoal() feedback.joint_names = goal.trajectory.joint_names watchdog = rospy.Timer(rospy.Duration( goal.trajectory.points[-1].time_from_start.to_sec() + 0.5), self._execution_timeout, oneshot=True) # Follow trajectory points goal_trajectory_point = goal.trajectory.points[-1] # Validate trajectory point if len(goal_trajectory_point.positions) != 1: result.error_code = result.INVALID_GOAL result.error_string = "Invalid joint position on trajectory point " self._joint_trajectory_action_server.set_aborted(result) return target_speed = 0.1 #target_speed = goal.trajectory.points[int(len(goal.trajectory.points)/2)].velocities[0] if goal.trajectory.points[int(len(goal.trajectory.points)/2)].velocities > 0 else 0.1 #print("Target gripper speed: " + target_speed) #target_speed = goal_trajectory_point.velocities[0] if len(goal_trajectory_point.velocities) > 0 else 0.01 target_force = goal_trajectory_point.effort[0] if len( goal_trajectory_point.effort) > 0 else 0.1 goal_command.position = self._driver.from_radians_to_distance( goal_trajectory_point.positions[0]) goal_command.speed = abs(target_speed) # To-Do: Convert to rad/s goal_command.force = target_force # Send incoming command to gripper driver self._driver.update_gripper_command(goal_command) # Set feedback desired value feedback.desired.positions = [goal_trajectory_point.positions[0]] while not rospy.is_shutdown( ) and self._processing_goal and not self._is_stalled: current_status = self._driver.get_current_gripper_status() feedback.actual.positions = [ self._driver.get_current_joint_position() ] error = abs(feedback.actual.positions[0] - feedback.desired.positions[0]) rospy.logdebug("Error : %.3f -- Actual: %.3f -- Desired: %.3f", error, self._driver.get_current_joint_position(), feedback.desired.positions[0]) feedback.error.positions = [error] self._joint_trajectory_action_server.publish_feedback(feedback) # Check for errors if current_status.fault_status != 0 and not self._is_stalled: self._is_stalled = True self._processing_goal = False rospy.logerr(msg) result.error_code = -6 result.error_string = "Gripper fault status (gFLT): " + current_status.fault_status self._joint_trajectory_action_server.set_aborted(result) return # Check if object was detected if current_status.obj_detected: watchdog.shutdown() # Stop timeout watchdog. self._processing_goal = False self._is_stalled = False result.error_code = result.SUCCESSFUL result.error_string = "Object detected/grasped" self._joint_trajectory_action_server.set_succeeded(result) return # Check if current trajectory point was reached if error < GOAL_DETECTION_THRESHOLD: break # Entire trajectory was followed/reached watchdog.shutdown() rospy.logdebug(self._action_name + ": goal reached") result.error_code = result.SUCCESSFUL result.error_string = "Goal reached" self._joint_trajectory_action_server.set_succeeded(result) self._processing_goal = False self._is_stalled = False
def publish_feedback(self): """Publishes the position feedback of the robot. The feedback is the difference between the desired states and the current ones. """ # If there is no trajectory, there is nothing to do if self.received_trajectory is None: return # Get the current states of the joints success, current_joints_states = \ self.robot_interface.get_current_joints() if not success: # Couldn't get the current joints' state rospy.logwarn("Could not publish on feedback_states:" "current states are unknown. Assuming all to 0.") current_joints_states = [0 for _ in range(6)] # Get the desired position. What should the robot joints be right now? time_from_start = rospy.Time.now() - self.start_time # Find which point represents the current desired position for point in self.received_trajectory.points: if time_from_start > point.time_from_start: continue break desired_point = point # Make sure the length of the current states and the target states # is exactly the length of the joints assert len(JOINTS_NAMES) == len(current_joints_states) and \ len(JOINTS_NAMES) == len(desired_point.positions), \ "Target and current states have different length. " \ "Expected {} joints, got {} (target) and {} (current)".format( len(JOINTS_NAMES), len(desired_point.positions), len(current_joints_states) ) # Create the message to be published msg = FollowJointTrajectoryFeedback() msg.header.frame_id = "" msg.header.stamp = rospy.get_rostime() msg.joint_names = JOINTS_NAMES # Set the goal states4 msg.desired.positions = desired_point.positions msg.desired.velocities = [] msg.desired.accelerations = [] msg.desired.effort = [] msg.desired.time_from_start = desired_point.time_from_start # Set the actual states msg.actual.positions = current_joints_states msg.actual.velocities = [] msg.actual.accelerations = [] msg.actual.effort = [] msg.actual.time_from_start = desired_point.time_from_start # Calculate the error position_error = [ goal - current for goal, current in zip(msg.desired.positions, msg.actual.positions) ] velocity_error = [ goal - current for goal, current in zip(msg.desired.velocities, msg.actual.velocities) ] acceleration_error = [ goal - current for goal, current in zip(msg.desired.accelerations, msg.actual.accelerations) ] effort_error = [ goal - current for goal, current in zip(msg.desired.effort, msg.actual.effort) ] # Set the errors msg.error.positions = position_error msg.error.velocities = velocity_error msg.error.accelerations = acceleration_error msg.error.effort = effort_error msg.error.time_from_start = desired_point.time_from_start # Publish the message on /feedback_states topic self.pub_feedback_states.publish(msg)