def start_talking(): while client.is_connected: talker.publish(Message({'data': 'Hello RobArch World!'})) print('Sending message...') time.sleep(1) talker.unadvertise()
def start_sending(): while True: if context['counter'] >= 3: break publisher.publish(Message({'data': 'hello world'})) time.sleep(0.1) publisher.unadvertise()
def ros_msg(self): msg = { "height": self.height, "width": self.width, "data": self.data, } return Message(msg)
def start_sending(): while True: if not ros_client.is_connected: break publisher.publish(Message({'data': 'hello world'})) time.sleep(0.1) publisher.unadvertise()
def ros_msg(self): msg = { "range_min": self.range_min, "range_max": self.range_max, "ranges": self.ranges, "intensities": self.intensities, } return Message(msg)
def _run_topic_pubsub(self): try: self._listener = Topic(self._ros_client, self._remote_topic_name, self._message_type) rospy.init_node('cloudsub', anonymous=True) self._ros_client.send_on_ready(Message(self._file)) global my my=rospy.get_time() self._ros_client.on_ready(self._start_receiving, run_in_thread=True) self._ros_client.run_event_loop() except: self._logger.exception("Fatal error in constructor")
def start_sending(): while True: print("---------------------") if not ros_client.is_connected: break publisher.publish(Message({'data': 'hello world'})) print(listener.is_subscribed) time.sleep(0.1) ros_client.terminate break # ros_client.get_topics(callback) publisher.unadvertise()
def ros_msg(self): msg = { "linear": { "x": self.x, "y": self.y, "z": self.z, }, "angular": { "x": self.xx, "y": self.yy, "z": self.zz, }, } return Message(msg)
def direct_ur_movel(ros_client, callback, frames, acceleration=None, velocity=None, time=None, radius=None): action_client = DirectUrActionClient(ros_client, timeout=50000) script_lines = [] for frame in frames: ptp = URPoseTrajectoryPoint(URPose.from_frame(frame), acceleration, velocity, time, radius) move = URMovel(ptp) script_lines.append(move) urgoal = URGoal(script_lines) goal = Goal(action_client, Message(urgoal.msg)) action_client.on('timeout', lambda: print('CLIENT TIMEOUT')) # goal.on('feedback', lambda feedback: print(feedback)) goal.on('result', callback) action_client.send_goal(goal)
def send_goal(self, goal, result_callback=None, timeout=None): """Send goal to the action server. Args: goal (:class:`URGoal`): The goal containing the script lines timeout (:obj:`int`): Timeout for the goal's result expressed in milliseconds. callback (:obj:`callable`): Function to be called when a result is received. It is a shorthand for hooking on the ``result`` event. """ self._internal_state == 'idle' if result_callback: goal.on('result', result_callback) ur_script = goal.goal_message['goal']['script'] message = Message({'data': ur_script}) self._urscript_topic.publish(message) if timeout: self.ros.call_later(timeout / 1000., goal._trigger_timeout)
def start_thread(self): while True: if self.ros_client.is_connected: self.publisher.publish( Message({ 'linear': { 'x': 0.5, 'y': 0, 'z': 0 }, 'angular': { 'x': 0, 'y': 0, 'z': 0.5 } })) else: print("Disconnect") break time.sleep(1.0)
def ros_msg(self): msg = { "pose": { "pose": { "position": { "x": self.x, "y": self.y, "z": self.z, }, "orientation": { "x": self._qx, "y": self._qy, "z": self._qz, "w": self._qw, }, }, "covariance": [], } } return Message(msg)
def _get_movement_ros_message(fw_step, r_step): frame_id = 'base_footprint' m = Message({ 'header': { 'frame_id': frame_id }, 'pose': { 'position': { 'x': fw_step, 'y': 0, 'z': 0 }, 'orientation': { 'x': 0, 'y': 0, 'z': r_step, 'w': 1 } } }) return m
def receive_message(self, message): # context['counter'] += 1 print('Receive data from Ros, ' , message['data']) #assert message['data'] == 'hello world', 'Unexpected message content' if(message['data'] == "action"): self.client.publish("topic/parameters", 'action') time.sleep(3) print("lastest status from left = " , self.status_left) print("lastest status from right = " , self.status_right) if self.status_left and not self.status_right: ##======================TURN LEFT self.action_mode = 1 elif not self.status_left and self.status_right: ##======================TURN RIGHT self.action_mode = 2 elif not self.status_left and not self.status_right: ##======================MOVE FORWARD self.action_mode = 3 elif self.status_left and self.status_right: ##======================MOVE BACKWARD self.action_mode = 4 print("summary action mode = ", str(self.action_mode)) self.client.publish("topic/parameters", 'wait') isConnected = self.ros_client.is_connected print("isConnected = ", isConnected) if(isConnected): self.publisher.publish(Message({'data': str(self.action_mode)}))
def execute_joint_trajectory(self, joint_trajectory, action_name='/execute_trajectory', callback=None, errback=None, feedback_callback=None, timeout=60000): """Execute a joint trajectory via the MoveIt infrastructure. Note ---- This method does not support Multi-DOF Joint Trajectories. Parameters ---------- joint_trajectory : :class:`compas_fab.robots.JointTrajectory` Instance of joint trajectory. callback : callable Function to be invoked when the goal is completed, requires one positional parameter ``result``. action_name : string ROS action name, defaults to ``/execute_trajectory``. errback : callable Function to be invoked in case of error or timeout, requires one position parameter ``exception``. feedback_callback : callable Function to be invoked during execution to provide feedback. timeout : int Timeout for goal completion in milliseconds. Returns ------- :class:`CancellableFutureResult` An instance of a cancellable future result. """ joint_trajectory = self._convert_to_ros_trajectory(joint_trajectory) trajectory = RobotTrajectory(joint_trajectory=joint_trajectory) trajectory_goal = ExecuteTrajectoryGoal(trajectory=trajectory) action_client = ActionClient(self, action_name, 'moveit_msgs/ExecuteTrajectoryAction') goal = Goal(action_client, Message(trajectory_goal.msg)) action_result = CancellableRosActionResult(goal) def handle_result(msg): result = ExecuteTrajectoryResult.from_msg(msg) if result.error_code != MoveItErrorCodes.SUCCESS: ros_error = RosError( 'Execute trajectory failed. Message={}'.format( result.error_code.human_readable), int(result.error_code)) action_result._set_error_result(ros_error) if errback: errback(ros_error) else: action_result._set_result(result) if callback: callback(result) def handle_feedback(msg): feedback = ExecuteTrajectoryFeedback.from_msg(msg) feedback_callback(feedback) def handle_failure(error): errback(error) goal.on('result', handle_result) if feedback_callback: goal.on('feedback', handle_feedback) if errback: goal.on( 'timeout', lambda: handle_failure(RosError('Action Goal timeout', -1))) goal.send(timeout=timeout) return action_result
def follow_joint_trajectory(self, joint_trajectory, action_name='/joint_trajectory_action', callback=None, errback=None, feedback_callback=None, timeout=60000): """Follow the joint trajectory as computed by MoveIt planner. Parameters ---------- joint_trajectory : :class:`compas_fab.robots.JointTrajectory` Instance of joint trajectory. Note: for backwards compatibility, this supports a ROS Msg being passed as well. action_name : string ROS action name, defaults to ``/joint_trajectory_action`` but some drivers need ``/follow_joint_trajectory``. callback : callable Function to be invoked when the goal is completed, requires one positional parameter ``result``. errback : callable Function to be invoked in case of error or timeout, requires one position parameter ``exception``. feedback_callback : callable Function to be invoked during execution to provide feedback. timeout : int Timeout for goal completion in milliseconds. Returns ------- :class:`CancellableTask` An instance of a cancellable tasks. """ joint_trajectory = self._convert_to_ros_trajectory(joint_trajectory) trajectory_goal = FollowJointTrajectoryGoal( trajectory=joint_trajectory) action_client = ActionClient( self, action_name, 'control_msgs/FollowJointTrajectoryAction') goal = Goal(action_client, Message(trajectory_goal.msg)) action_result = CancellableRosActionResult(goal) def handle_result(msg): result = FollowJointTrajectoryResult.from_msg(msg) if result.error_code != FollowJointTrajectoryResult.SUCCESSFUL: ros_error = RosError( 'Follow trajectory failed={}'.format(result.error_string), int(result.error_code)) action_result._set_error_result(ros_error) if errback: errback(ros_error) else: action_result._set_result(result) if callback: callback(result) def handle_feedback(msg): feedback = FollowJointTrajectoryFeedback.from_msg(msg) feedback_callback(feedback) def handle_failure(error): errback(error) goal.on('result', handle_result) if feedback_callback: goal.on('feedback', handle_feedback) if errback: goal.on( 'timeout', lambda: handle_failure(RosError('Action Goal timeout', -1))) goal.send(timeout=timeout) return action_result
def ros_msg(self) -> Message: return Message({})
import time from roslibpy import Message from roslibpy import Topic from compas_fab.backends import RosClient with RosClient('localhost') as client: talker = Topic(client, '/chatter', 'std_msgs/String') talker.advertise() while client.is_connected: talker.publish(Message({'data': 'Hello World!'})) print('Sending message...') time.sleep(1) talker.unadvertise()
def follow_joint_trajectory(self, joint_trajectory, callback=None, errback=None, feedback_callback=None, timeout=60000): """Follow the joint trajectory as computed by MoveIt planner. Parameters ---------- joint_trajectory : JointTrajectory Joint trajectory message as computed by MoveIt planner callback : callable Function to be invoked when the goal is completed, requires one positional parameter ``result``. errback : callable Function to be invoked in case of error or timeout, requires one position parameter ``exception``. feedback_callback : callable Function to be invoked during execution to provide feedback. timeout : int Timeout for goal completion in milliseconds. Returns ------- :class:`CancellableTask` An instance of a cancellable tasks. """ trajectory_goal = FollowJointTrajectoryGoal( trajectory=joint_trajectory) def handle_result(msg, client): result = FollowJointTrajectoryResult.from_msg(msg) callback(result) def handle_failure(error): errback(error) connection_timeout = 3000 action_client = ActionClient( self, '/joint_trajectory_action', # '/follow_joint_trajectory', 'control_msgs/FollowJointTrajectoryAction', connection_timeout) goal = Goal(action_client, Message(trajectory_goal.msg)) if callback: goal.on('result', lambda result: handle_result(result, action_client)) if feedback_callback: goal.on('feedback', feedback_callback) if errback: goal.on( 'timeout', lambda: handle_failure(RosError("Action Goal timeout", -1))) action_client.on( 'timeout', lambda: handle_failure( RosError("Actionlib client timeout", -1))) goal.send(timeout=timeout) return CancellableRosAction(goal)