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)
