diff --git a/industrial_robot_simulator/industrial_robot_simulator b/industrial_robot_simulator/industrial_robot_simulator index ebc9519c..62c90078 100755 --- a/industrial_robot_simulator/industrial_robot_simulator +++ b/industrial_robot_simulator/industrial_robot_simulator @@ -8,16 +8,15 @@ import Queue # Publish from sensor_msgs.msg import JointState -from control_msgs.msg import FollowJointTrajectoryFeedback +from control_msgs.msg import FollowJointTrajectoryFeedback +from industrial_msgs.msg import RobotStatus # Subscribe from trajectory_msgs.msg import JointTrajectory from trajectory_msgs.msg import JointTrajectoryPoint # Reference -from industrial_msgs.msg import TriState -import trajectory_msgs - +from industrial_msgs.msg import TriState, RobotMode @@ -31,64 +30,60 @@ This class IS threadsafe """ class MotionControllerSimulator(): - """ Constructor of motion controller simulator """ def __init__(self, num_joints, initial_joint_state, update_rate = 100, buffer_size = 0): - # Class lock self.lock = threading.Lock() - + # Motion loop update rate (higher update rates result in smoother simulated motion) self.update_rate = update_rate rospy.loginfo("Setting motion update rate (hz): %f", self.update_rate) - + # Initialize joint position self.joint_positions = initial_joint_state rospy.loginfo("Setting initial joint state: %s", str(initial_joint_state)) - + # Initialize motion buffer (contains joint position lists) self.motion_buffer = Queue.Queue(buffer_size) rospy.loginfo("Setting motion buffer size: %i", buffer_size) - + # Shutdown signal self.sig_shutdown = False - + # Stop signal self.sig_stop = False - + # Motion thread self.motion_thread = threading.Thread(target=self._motion_worker) self.motion_thread.daemon = True self.motion_thread.start() - + """ """ def add_motion_waypoint(self, point): - self.motion_buffer.put(point) - - + + """ """ def get_joint_positions(self): - with self.lock: return self.joint_positions[:] - + """ """ def is_in_motion(self): return not self.motion_buffer.empty() - + """ """ def shutdown(self): self.sig_shutdown = True rospy.loginfo('Motion_Controller shutdown signaled') - + """ """ def stop(self): @@ -96,7 +91,7 @@ class MotionControllerSimulator(): with self.lock: self._clear_buffer() self.sig_stop = True - + """ """ def interpolate(self, last_pt, current_pt, alpha): @@ -105,19 +100,18 @@ class MotionControllerSimulator(): intermediate_pt.positions.append(last_joint + alpha*(current_joint-last_joint)) intermediate_pt.time_from_start = last_pt.time_from_start + rospy.Duration(alpha*(current_pt.time_from_start.to_sec() - last_pt.time_from_start.to_sec())) return intermediate_pt - + """ """ def _clear_buffer(self): with self.motion_buffer.mutex: self.motion_buffer.queue.clear() - + """ """ def _move_to(self, point, dur): - rospy.sleep(dur) - + with self.lock: if not self.sig_stop: self.joint_positions = point.positions[:] @@ -125,26 +119,23 @@ class MotionControllerSimulator(): else: rospy.loginfo('Stoping motion immediately, clearing stop signal') self.sig_stop = False - + """ """ def _motion_worker(self): - rospy.loginfo('Starting motion worker in motion controller simulator') move_duration = rospy.Duration() if self.update_rate <> 0.: update_duration = rospy.Duration(1./self.update_rate) last_goal_point = JointTrajectoryPoint() - + with self.lock: last_goal_point.positions = self.joint_positions[:] - + while not self.sig_shutdown: - try: - current_goal_point = self.motion_buffer.get() - + # If the current time from start is less than the last, then it's a new trajectory if current_goal_point.time_from_start < last_goal_point.time_from_start: move_duration = current_goal_point.time_from_start @@ -161,17 +152,15 @@ class MotionControllerSimulator(): move_duration = current_goal_point.time_from_start - intermediate_goal_point.time_from_start self._move_to(current_goal_point, move_duration) - last_goal_point = copy.deepcopy(current_goal_point) - + except Exception as e: rospy.logerr('Unexpected exception: %s', e) - - + rospy.loginfo("Shutting down motion controller") - - - + + + """ IndustrialRobotSimulator @@ -185,14 +174,11 @@ TODO: Currently the simulator only supports the bare minimum motion interface. TODO: Interfaces to add: -Robot status Joint streaming All services """ class IndustrialRobotSimulatorNode(): - - """ Constructor of industrial robot simulator """ @@ -201,18 +187,18 @@ class IndustrialRobotSimulatorNode(): # Class lock self.lock = threading.Lock() - + # Publish rate (hz) self.pub_rate = rospy.get_param('pub_rate', 10.0) rospy.loginfo("Setting publish rate (hz) based on parameter: %f", self.pub_rate) - + # Joint names def_joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"] self.joint_names = rospy.get_param('controller_joint_names', def_joint_names) if len(self.joint_names) == 0: rospy.logwarn("Joint list is empty, did you set controller_joint_name?") rospy.loginfo("Setting joint names based on parameter: %s", str(self.joint_names)) - + # Setup initial joint positions num_joints = len(self.joint_names) initial_joint_state = rospy.get_param('initial_joint_state', [0]*num_joints) @@ -230,68 +216,95 @@ class IndustrialRobotSimulatorNode(): # Published to joint states rospy.loginfo("Creating joint state publisher") self.joint_state_pub = rospy.Publisher('joint_states', JointState) - + # Published to joint feedback rospy.loginfo("Creating joint feedback publisher") self.joint_feedback_pub = rospy.Publisher('feedback_states', FollowJointTrajectoryFeedback) + rospy.loginfo("Creating robot status publisher") + self.robot_status_pub = rospy.Publisher('robot_status', RobotStatus) + # Subscribe to a joint trajectory rospy.loginfo("Creating joint trajectory subscriber") self.joint_path_sub = rospy.Subscriber('joint_path_command', JointTrajectory, self.trajectory_callback) - # Timed task (started automatically) + # JointStates timed task (started automatically) period = rospy.Duration(1.0/self.pub_rate) - rospy.loginfo('Setting up publish worker with period (sec): %s', str(period.to_sec())) + rospy.loginfo('Setting up joint state publish worker with period (sec): %s', str(period.to_sec())) rospy.Timer(period, self.publish_worker) - + # Clean up init rospy.on_shutdown(self.motion_ctrl.shutdown) - - """ - The publish worker is executed at a fixed rate. The publishes the various + The publish worker is executed at a fixed rate. This publishes the various state and status information for the robot. """ def publish_worker(self, event): self.joint_state_publisher() - - + self.robot_status_publisher() + + """ The joint state publisher publishes the current joint state and the current - feedback state (as these are closely releated) - """ + feedback state (as these are closely related) + """ 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) - - + + + """ + The robot status publisher publishes the current simulated robot status. + + The following values are hard coded: + - robot always in AUTO mode + - drives always powered + - motion always possible + - robot never E-stopped + - no error code + - robot never in error + + The value of 'in_motion' is derived from the state of the + MotionControllerSimulator. + """ + def robot_status_publisher(self): + try: + msg = RobotStatus() + msg.mode.val = RobotMode.AUTO + msg.e_stopped.val = TriState.FALSE + msg.drives_powered.val = TriState.TRUE + msg.motion_possible.val = TriState.TRUE + msg.in_motion.val = self.motion_ctrl.is_in_motion() + msg.in_error.val = TriState.FALSE + msg.error_code = 0 + self.robot_status_pub.publish(msg) + + except Exception as e: + rospy.logerr('Unexpected exception: %s', e) + + """ Trajectory subscription callback (gets called whenever a joint trajectory is received). @@ -300,29 +313,24 @@ class IndustrialRobotSimulatorNode(): @type msg_in: JointTrajectory """ def trajectory_callback(self, msg_in): - try: - rospy.loginfo('Received trajectory with %s points, executing callback', str(len(msg_in.points))) - + if self.motion_ctrl.is_in_motion(): if len(msg_in.points) > 0: rospy.logerr('Received trajectory while still in motion, trajectory splicing not supported') else: rospy.loginfo('Received empty trajectory while still in motion, stopping current trajectory') self.motion_ctrl.stop() - + else: - for point in msg_in.points: - point = self._to_controller_order(msg_in.joint_names, point) self.motion_ctrl.add_motion_waypoint(point) - + except Exception as e: rospy.logerr('Unexpected exception: %s', e) - rospy.loginfo('Exiting trajectory callback') @@ -333,44 +341,36 @@ class IndustrialRobotSimulatorNode(): @type keys: list @param point: joint trajectory point @type point: JointTrajectoryPoint - + @return point: reorder point @type point: JointTrajectoryPoint """ def _to_controller_order(self, keys, point): - #rospy.loginfo('to controller order, keys: %s, point: %s', str(keys), str(point)) pt_rtn = copy.deepcopy(point) pt_rtn.positions = self._remap_order(self.joint_names, keys, point.positions) - + return pt_rtn - + def _remap_order(self, ordered_keys, value_keys, values): - #rospy.loginfo('remap order, ordered_keys: %s, value_keys: %s, values: %s', str(ordered_keys), str(value_keys), str(values)) ordered_values = [] - ordered_values = [0]*len(ordered_keys) mapping = dict(zip(value_keys, values)) #rospy.loginfo('maping: %s', str(mapping)) - + for i in range(len(ordered_keys)): ordered_values[i] = mapping[ordered_keys[i]] pass return ordered_values - - - - if __name__ == '__main__': try: rospy.loginfo('Executing joint_controller_simulator') controller = IndustrialRobotSimulatorNode() rospy.spin() - except rospy.ROSInterruptException: pass - - + except rospy.ROSInterruptException: + pass