def _attached_collision_object(self, attached_collision_object, operation=CollisionObject.ADD): attached_collision_object.object.operation = operation topic = Topic(self, '/attached_collision_object', 'moveit_msgs/AttachedCollisionObject') topic.publish(attached_collision_object.msg)
def RunScript(self, ros_client, topic_name, topic_type, msg): if not topic_name: raise ValueError('Please specify the name of the topic') if not topic_type: raise ValueError('Please specify the type of the topic') key = create_id(self, 'topic') topic = st.get(key, None) if ros_client and ros_client.is_connected: if not topic: topic = Topic(ros_client, topic_name, topic_type) topic.advertise() time.sleep(0.2) st[key] = topic self.is_advertised = topic and topic.is_advertised if msg: msg = ROSmsg.parse(msg, topic_type) topic.publish(msg.msg) self.Message = 'Message published' else: if self.is_advertised: self.Message = 'Topic advertised' return (topic, self.is_advertised)
def collision_mesh(self, id_name, root_link, compas_mesh, operation=0): """ """ co = self.build_collision_object(root_link, id_name, compas_mesh, operation) topic = Topic(self, '/collision_object', 'moveit_msgs/CollisionObject') topic.publish(co.msg)
def stop(self, forced: bool = False) -> None: """Stop the robot. This is a blocking call. It will block execution until the robot is gracefully stopped unless `forced` is set to `True`. Args: forced (bool): Forcefully stop the robot or not. """ msg = Twist() t = Topic(self.ros(), "/cmd_vel", msg.ros_type()) t.publish(msg.ros_msg())
def attached_collision_mesh(self, id_name, ee_link, compas_mesh, operation=1, touch_links=None): """ """ aco = self.build_attached_collision_mesh(ee_link, id_name, compas_mesh, operation, touch_links=None) topic = Topic(self, '/attached_collision_object', 'moveit_msgs/AttachedCollisionObject') topic.publish(aco.msg)
def rotate(self, angle: float = 0, duration: float = 0, speed: float = 0) -> None: """Rotate the robot at a certain angle at a certain speed or for a certain duration. To rotate clockwise, set the speed to positive. To rotate in anti-clockwise, set the speed to negative. If the given speed is larger than the maximum speed, it will be set to the maximum speed. Args: angle (float): Angle to rotate (in degrees). duration (float): Time duration to rotate for (in seconds). speed (float): Rotation speed (radian per seconds). """ freq = DEFAULT_LOOP_FREQUENCY msg = Twist() t = Topic(self.ros(), "/cmd_vel", msg.ros_type()) # Rotate at $speed for $angle if speed > 0 and angle != 0: rad = np.deg2rad(angle) duration = abs(rad) / speed # Rotate for $duration to $angle if duration > 0 and angle != 0: rad = np.deg2rad(angle) speed = rad / duration # Rotate for $duration at $speed if duration > 0 and speed != 0: msg.zz = speed end_time = time.time() + duration while time.time() <= end_time: t.publish(msg.ros_msg()) time.sleep(1 / freq) self.stop()
class rosbridge_client: def __init__(self): self.ros_client = Ros('127.0.0.1', 9090) print("wait for server") self.publisher = Topic(self.ros_client, '/cmd_vel', 'geometry_msgs/Twist') self.listener = Topic(self.ros_client, '/odom', 'nav_msgs/Odometry') self.listener.subscribe(self.callback) self.ros_client.on_ready(self.start_thread, run_in_thread=True) self.ros_client.run_forever() def callback(self, message): x = message["pose"]["pose"]["position"]["x"] y = message["pose"]["pose"]["position"]["y"] print(x, y) 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 move(self, distance: float = 0, duration: float = 0, speed: float = 0) -> None: """Move the robot at a certain distance at a certain speed or for a certain duration. To move backwards, set speed to negative. If the given speed is larger than the maximum speed, it will be set to the maximum speed. Args: distance (float): Distance to travel. duration (float): Time duration to travel for (in seconds). speed (float): Travel speed. """ freq = DEFAULT_LOOP_FREQUENCY msg = Twist() t = Topic(self.ros(), "/cmd_vel", msg.ros_type()) # Move at $speed for $distance if speed > 0 and distance != 0: duration = distance / speed # Move for $duration to $distance if duration > 0 and distance != 0: speed = distance / duration # Move for $duration at $speed if duration > 0 and speed != 0: msg.x = speed end_time = time.time() + duration while time.time() <= end_time: t.publish(msg.ros_msg()) time.sleep(1 / freq) self.stop()
class Muse_app: def __init__(self): self.client = mqtt.Client() self.client2 = mqtt.Client() self.ros_client = Ros(host='10.205.240.34', port=9090) self.publisher = Topic(self.ros_client, '/chatter', 'std_msgs/String') self.muse_left = "DC11" self.muse_right = "C00E" self.status_left = False self.status_right = False self.action_mode = 0 self.client.on_connect = self.on_connect self.client.on_message = self.on_message self.client.connect("localhost", 1883, 60) t = threading.Thread(target=self.worker) t.start() self.client2.on_connect = self.on_connect2 self.client2.on_message = self.on_message self.client2.connect("localhost", 1883, 60) t2 = threading.Thread(target=self.worker2) t2.start() self.ros_client.on_ready(self.subscribing, run_in_thread=True) t3 = threading.Thread(target=self.worker3) t3.start() while True: pass def on_connect(self, client, userdata, flags, rc): print("Connected with result code "+str(rc)) # Subscribing in on_connect() means that if we lose the connection and # reconnect then subscriptions will be renewed. self.client.subscribe("topic/" + self.muse_left) #self.client.subscribe("topic/test") def on_connect2(self, client, userdata, flags, rc): print("Connected with result code "+str(rc)) # Subscribing in on_connect() means that if we lose the connection and # reconnect then subscriptions will be renewed. self.client2.subscribe("topic/" + self.muse_right) def worker(self): self.client.loop_forever() return def worker2(self): self.client2.loop_forever() return def on_message(self, client, userdata, message): print("message received " ,str(message.payload.decode("utf-8"))) print("message topic=",message.topic) if message.topic == "topic/" + self.muse_left: if(message.payload.decode("utf-8") == '0'): self.status_left = False elif(message.payload.decode("utf-8") == '1'): self.status_left = True print("left = " , self.status_left) if message.topic == "topic/" + self.muse_right: if(message.payload.decode("utf-8") == '0'): self.status_right = False elif(message.payload.decode("utf-8") == '1'): self.status_right = True print("right = " , self.status_right) # print("message qos=",message.qos) # print("message retain flag=",message.retain) def subscribing(self): print("subscribe to Ros..") self.publisher.subscribe(self.receive_message) def uninitialize(self): # nop return 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 worker3(self): self.ros_client.run_forever()
class MoveItPlanner(PlannerBackend): """Implement the planner backend interface based on MoveIt! """ GET_POSITION_IK = ServiceDescription('/compute_ik', 'GetPositionIK', GetPositionIKRequest, GetPositionIKResponse, validate_response) GET_POSITION_FK = ServiceDescription('/compute_fk', 'GetPositionFK', GetPositionFKRequest, GetPositionFKResponse, validate_response) GET_CARTESIAN_PATH = ServiceDescription('/compute_cartesian_path', 'GetCartesianPath', GetCartesianPathRequest, GetCartesianPathResponse, validate_response) GET_MOTION_PLAN = ServiceDescription('/plan_kinematic_path', 'GetMotionPlan', MotionPlanRequest, MotionPlanResponse, validate_response) GET_PLANNING_SCENE = ServiceDescription('/get_planning_scene', 'GetPlanningScene', GetPlanningSceneRequest, GetPlanningSceneResponse) def init_planner(self): self.collision_object_topic = Topic(self, '/collision_object', 'moveit_msgs/CollisionObject', queue_size=None) self.collision_object_topic.advertise() self.attached_collision_object_topic = Topic( self, '/attached_collision_object', 'moveit_msgs/AttachedCollisionObject', queue_size=None) self.attached_collision_object_topic.advertise() def dispose_planner(self): if hasattr(self, 'collision_object_topic') and self.collision_object_topic: self.collision_object_topic.unadvertise() if hasattr(self, 'attached_collision_object_topic' ) and self.attached_collision_object_topic: self.attached_collision_object_topic.unadvertise() def _convert_constraints_to_rosmsg(self, constraints, header): """Convert COMPAS FAB constraints into ROS Messages.""" if not constraints: return None ros_constraints = Constraints() for c in constraints: if c.type == c.JOINT: ros_constraints.joint_constraints.append( JointConstraint.from_joint_constraint(c)) elif c.type == c.POSITION: ros_constraints.position_constraints.append( PositionConstraint.from_position_constraint(header, c)) elif c.type == c.ORIENTATION: ros_constraints.orientation_constraints.append( OrientationConstraint.from_orientation_constraint( header, c)) else: raise NotImplementedError return ros_constraints # ========================================================================== # planning services # ========================================================================== def inverse_kinematics_async(self, callback, errback, robot, frame, group, start_configuration, avoid_collisions=True, constraints=None, attempts=8, attached_collision_meshes=None): """Asynchronous handler of MoveIt IK service.""" base_link = robot.model.root.name header = Header(frame_id=base_link) pose = Pose.from_frame(frame) pose_stamped = PoseStamped(header, pose) joint_state = JointState(name=start_configuration.joint_names, position=start_configuration.values, header=header) start_state = RobotState(joint_state, MultiDOFJointState(header=header)) if attached_collision_meshes: for acm in attached_collision_meshes: aco = AttachedCollisionObject.from_attached_collision_mesh(acm) start_state.attached_collision_objects.append(aco) constraints = self._convert_constraints_to_rosmsg(constraints, header) ik_request = PositionIKRequest(group_name=group, robot_state=start_state, constraints=constraints, pose_stamped=pose_stamped, avoid_collisions=avoid_collisions, attempts=attempts) def convert_to_positions(response): callback((response.solution.joint_state.position, response.solution.joint_state.name)) self.GET_POSITION_IK(self, (ik_request, ), convert_to_positions, errback) def forward_kinematics_async(self, callback, errback, robot, configuration, group, ee_link): """Asynchronous handler of MoveIt FK service.""" base_link = robot.model.root.name header = Header(frame_id=base_link) fk_link_names = [ee_link] joint_state = JointState(name=configuration.joint_names, position=configuration.values, header=header) robot_state = RobotState(joint_state, MultiDOFJointState(header=header)) def convert_to_frame(response): callback(response.pose_stamped[0].pose.frame) self.GET_POSITION_FK(self, (header, fk_link_names, robot_state), convert_to_frame, errback) def plan_cartesian_motion_async(self, callback, errback, robot, frames, start_configuration, group, max_step, jump_threshold, avoid_collisions, path_constraints, attached_collision_meshes): """Asynchronous handler of MoveIt cartesian motion planner service.""" base_link = robot.model.root.name # use world coords ee_link = robot.get_end_effector_link_name(group) header = Header(frame_id=base_link) waypoints = [Pose.from_frame(frame) for frame in frames] joint_state = JointState(header=header, name=start_configuration.joint_names, position=start_configuration.values) start_state = RobotState(joint_state, MultiDOFJointState(header=header)) if attached_collision_meshes: for acm in attached_collision_meshes: aco = AttachedCollisionObject.from_attached_collision_mesh(acm) start_state.attached_collision_objects.append(aco) path_constraints = self._convert_constraints_to_rosmsg( path_constraints, header) request = dict(header=header, start_state=start_state, group_name=group, link_name=ee_link, waypoints=waypoints, max_step=float(max_step), jump_threshold=float(jump_threshold), avoid_collisions=bool(avoid_collisions), path_constraints=path_constraints) def convert_to_trajectory(response): try: trajectory = JointTrajectory() trajectory.source_message = response trajectory.fraction = response.fraction trajectory.joint_names = response.solution.joint_trajectory.joint_names joint_types = robot.get_joint_types_by_names( trajectory.joint_names) trajectory.points = convert_trajectory_points( response.solution.joint_trajectory.points, joint_types) start_state = response.start_state.joint_state start_state_types = robot.get_joint_types_by_names( start_state.name) trajectory.start_configuration = Configuration( start_state.position, start_state_types, start_state.name) callback(trajectory) except Exception as e: errback(e) self.GET_CARTESIAN_PATH(self, request, convert_to_trajectory, errback) def plan_motion_async(self, callback, errback, robot, goal_constraints, start_configuration, group, path_constraints=None, trajectory_constraints=None, planner_id='', num_planning_attempts=8, allowed_planning_time=2., max_velocity_scaling_factor=1., max_acceleration_scaling_factor=1., attached_collision_meshes=None, workspace_parameters=None): """Asynchronous handler of MoveIt motion planner service.""" # http://docs.ros.org/jade/api/moveit_core/html/utils_8cpp_source.html # TODO: if list of frames (goals) => receive multiple solutions? base_link = robot.model.root.name # use world coords header = Header(frame_id=base_link) joint_state = JointState(header=header, name=start_configuration.joint_names, position=start_configuration.values) start_state = RobotState(joint_state, MultiDOFJointState(header=header)) if attached_collision_meshes: for acm in attached_collision_meshes: aco = AttachedCollisionObject.from_attached_collision_mesh(acm) start_state.attached_collision_objects.append(aco) # convert constraints goal_constraints = [ self._convert_constraints_to_rosmsg(goal_constraints, header) ] path_constraints = self._convert_constraints_to_rosmsg( path_constraints, header) if trajectory_constraints is not None: trajectory_constraints = TrajectoryConstraints( constraints=self._convert_constraints_to_rosmsg( path_constraints, header)) request = dict( start_state=start_state, goal_constraints=goal_constraints, path_constraints=path_constraints, trajectory_constraints=trajectory_constraints, planner_id=planner_id, group_name=group, num_planning_attempts=num_planning_attempts, allowed_planning_time=allowed_planning_time, max_velocity_scaling_factor=max_velocity_scaling_factor, max_acceleration_scaling_factor=max_velocity_scaling_factor) # workspace_parameters=workspace_parameters def convert_to_trajectory(response): trajectory = JointTrajectory() trajectory.source_message = response trajectory.fraction = 1. trajectory.joint_names = response.trajectory.joint_trajectory.joint_names trajectory.planning_time = response.planning_time joint_types = robot.get_joint_types_by_names( trajectory.joint_names) trajectory.points = convert_trajectory_points( response.trajectory.joint_trajectory.points, joint_types) start_state = response.trajectory_start.joint_state start_state_types = robot.get_joint_types_by_names( start_state.name) trajectory.start_configuration = Configuration( start_state.position, start_state_types, start_state.name) callback(trajectory) self.GET_MOTION_PLAN(self, request, convert_to_trajectory, errback) # ========================================================================== # collision objects # ========================================================================== def get_planning_scene_async(self, callback, errback): request = dict(components=PlanningSceneComponents( PlanningSceneComponents.SCENE_SETTINGS | PlanningSceneComponents.ROBOT_STATE | PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS | PlanningSceneComponents.WORLD_OBJECT_NAMES | PlanningSceneComponents.WORLD_OBJECT_GEOMETRY | PlanningSceneComponents.ALLOWED_COLLISION_MATRIX | PlanningSceneComponents.OBJECT_COLORS)) self.GET_PLANNING_SCENE(self, request, callback, errback) def add_collision_mesh(self, collision_mesh): """Add a collision mesh to the planning scene.""" co = CollisionObject.from_collision_mesh(collision_mesh) self._collision_object(co, CollisionObject.ADD) def remove_collision_mesh(self, id): """Remove a collision mesh from the planning scene.""" co = CollisionObject() co.id = id self._collision_object(co, CollisionObject.REMOVE) def append_collision_mesh(self, collision_mesh): """Append a collision mesh to the planning scene.""" co = CollisionObject.from_collision_mesh(collision_mesh) self._collision_object(co, CollisionObject.APPEND) def _collision_object(self, collision_object, operation=CollisionObject.ADD): if not hasattr( self, 'collision_object_topic') or not self.collision_object_topic: self.init_planner() collision_object.operation = operation self.collision_object_topic.publish(collision_object.msg) def add_attached_collision_mesh(self, attached_collision_mesh): """Add a collision mesh attached to the robot.""" aco = AttachedCollisionObject.from_attached_collision_mesh( attached_collision_mesh) self._attached_collision_object(aco, operation=CollisionObject.ADD) def remove_attached_collision_mesh(self, id): """Add an attached collision mesh from the robot.""" aco = AttachedCollisionObject() aco.object.id = id return self._attached_collision_object( aco, operation=CollisionObject.REMOVE) def _attached_collision_object(self, attached_collision_object, operation=CollisionObject.ADD): if not hasattr(self, 'attached_collision_object_topic' ) or not self.attached_collision_object_topic: self.init_planner() attached_collision_object.object.operation = operation self.attached_collision_object_topic.publish( attached_collision_object.msg)
import time from roslibpy import Topic from compas_fab.backends import RosClient client = RosClient() client.run() topic = Topic(client, '/messages', 'std_msgs/String') topic.advertise() while True: topic.publish(dict(data='Hello world')) time.sleep(1) client.terminate()
class DirectUrActionClient(EventEmitterMixin): """Direct UR Script action client to simulate an action interface on arbitrary URScript commands. """ def __init__(self, ros, timeout=None, omit_feedback=False, omit_status=False, omit_result=False): super(DirectUrActionClient, self).__init__() self.server_name = '/ur_driver/URScript' self.ros = ros self.timeout = timeout self.omit_feedback = omit_feedback self.omit_status = omit_status self.omit_result = omit_result self.goal = None self._received_status = False # Advertise the UR Script topic self._urscript_topic = Topic(ros, self.server_name, 'std_msgs/String') self._urscript_topic.advertise() # Create the topics associated with actionlib self.status_listener = Topic(ros, '/tool_velocity', 'geometry_msgs/TwistStamped') # Subscribe to the status topic if not self.omit_status: self.status_listener.subscribe(self._on_status_message) # If timeout specified, emit a 'timeout' event if the action server does not respond if self.timeout: self.ros.call_later(self.timeout / 1000., self._trigger_timeout) self._internal_state = 'idle' def _on_status_message(self, message): self._received_status = True self.goal.emit('status', message) twist = message['twist'] total_velocity = math.fsum(list(twist['linear'].values()) + list(twist['angular'].values())) in_motion = total_velocity != 0.0 if self._internal_state == 'idle': if in_motion: self._internal_state = 'executing_goal' elif self._internal_state == 'executing_goal': if not in_motion: self._internal_state = 'stopped' self.goal.emit('result', message) def _trigger_timeout(self): if not self._received_status: self.emit('timeout') def add_goal(self, goal): """Add a goal to this action client. Args: goal (:class:`.Goal`): Goal to add. """ self.goal = goal def cancel(self): """Cancel all goals associated with this action client.""" pass # self.cancel_topic.publish(Message()) def dispose(self): """Unsubscribe and unadvertise all topics associated with this action client.""" # And the UR Script topic self._urscript_topic.unadvertise() if not self.omit_status: self.status_listener.unsubscribe(self._on_status_message) 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)
class RemoteWidget(QWidget): def __init__(self, ros: Ros, parent=None): super(RemoteWidget, self).__init__(parent) self.ros_client = ros layout = QHBoxLayout() left_layout = QVBoxLayout() left_layout.addWidget(HDevider()) self._step_slider = RemoteSlider('前进量', -40, 40, 1000.0) left_layout.addWidget(self._step_slider) self._lateral_slider = RemoteSlider('横移量', -30, 30, 1000.0) left_layout.addWidget(self._lateral_slider) self._turn_slider = RemoteSlider('偏转量', -20, 20, 1.0) left_layout.addWidget(self._turn_slider) self._run_button = QPushButton('执行') self._run_button.clicked.connect(self.on_run_clicked) blayout1 = QHBoxLayout() blayout1.addWidget(self._run_button) left_layout.addLayout(blayout1) left_layout.addWidget(HDevider()) self._head_yaw_slider = RemoteSlider('头部方向', -120, 120, 1.0) self._head_yaw_slider.valueChanged.connect(self.on_head_task) left_layout.addWidget(self._head_yaw_slider) self._head_pitch_slider = RemoteSlider('头部俯仰', -90, 90, 1.0) self._head_pitch_slider.valueChanged.connect(self.on_head_task) left_layout.addWidget(self._head_pitch_slider) left_layout.addWidget(HDevider()) led_layout = QHBoxLayout() self._led1_switch = ToggleSwitch('LED1') self._led1_switch.toggled.connect(self.on_led_task) self._led2_switch = ToggleSwitch('LED2') self._led2_switch.toggled.connect(self.on_led_task) self._imu_reset_btn = QPushButton('IMU重置') self._imu_reset_btn.clicked.connect(self.on_imu_reset) led_layout.addWidget(self._led1_switch) led_layout.addWidget(self._led2_switch) led_layout.addWidget(self._imu_reset_btn) left_layout.addLayout(led_layout) left_layout.addWidget(HDevider()) layout.addLayout(left_layout) right_layout = QVBoxLayout() self._acts_widget = QListWidget() self._acts_widget.setStyleSheet( "QListWidget::item { border-bottom: 1px solid black; }") self._acts_widget.itemDoubleClicked.connect(self.on_act_task) right_layout.addWidget(self._acts_widget) self._load_act_button = QPushButton('加载动作列表') self._load_act_button.clicked.connect(self.load_actions) right_layout.addWidget(self._load_act_button) layout.addLayout(right_layout) self.setLayout(layout) self._timer = QTimer() self._timer.timeout.connect(self.on_walk_task) self._timer.start(600) self._walk = False self._led_task = Topic(self.ros_client, '/task/led', 'common/LedTask') self._led_task.advertise() self._head_task = Topic(self.ros_client, '/task/head', 'common/HeadTask') self._head_task.advertise() self._body_task = Topic(self.ros_client, '/task/body', 'common/BodyTask') self._body_task.advertise() def check_connect(self): if self.ros_client is None: QMessageBox.critical(self, "错误", '未连接ROS') return False if not self.ros_client.is_connected: QMessageBox.critical(self, "错误", '连接中断,请重新连接') if self._walk: self._run_button.clicked.emit(True) return False return True def load_actions(self): self._acts_widget.clear() if not self.check_connect(): return try: service = Service(self.ros_client, '/get_actions', 'common/GetActions') result = service.call(ServiceRequest({})) actions = result['actions'] self._acts_widget.clear() for action in actions: litem = QListWidgetItem() aitem = QLabel(action) aitem.setFixedHeight(30) aitem.setStyleSheet('QLabel{margin-left: 5px;}') aitem.show() self._acts_widget.addItem(litem) self._acts_widget.setItemWidget(litem, aitem) litem.setSizeHint( QSize(aitem.rect().width(), aitem.rect().height())) except Exception as e: QMessageBox.critical(self, "错误", e.args[0]) def on_run_clicked(self): if not self.check_connect(): return self._walk = not self._walk self._run_button.setText('停止' if self._walk else '执行') def on_imu_reset(self): if not self.check_connect(): return try: service = Service(self.ros_client, '/imu_reset', 'std_srvs/Empty') service.call(ServiceRequest({})) except Exception as e: QMessageBox.critical(self, "错误", e.args[0]) def on_led_task(self): if not self.check_connect(): return self._led_task.publish({ 'led1': self._led1_switch.switch(), 'led2': self._led2_switch.switch() }) def on_head_task(self): if not self.check_connect(): return self._head_task.publish({ 'mode': 3, 'yaw': self._head_yaw_slider.value(), 'pitch': self._head_pitch_slider.value() }) def on_walk_task(self): if self.ros_client is None or not self.ros_client.is_connected: return if self._walk and self.ros_client.is_connected: self._body_task.publish({ 'type': 1, 'actname': '', 'count': 2, 'step': self._step_slider.value(), 'lateral': self._lateral_slider.value(), 'turn': self._turn_slider.value() }) def on_act_task(self): # double click on action! if not self.check_connect(): return item = self._acts_widget.currentItem() if item is None: return aitem = self._acts_widget.itemWidget(item) aitem.__class__ = QLabel action = aitem.text() if self._walk: self.on_run_clicked() self._body_task.publish({ 'type': 2, 'actname': action, 'count': 1, 'step': 0, 'lateral': 0, 'turn': 0 })
import time from roslibpy import Topic from compas_fab.backends import RosClient with RosClient('localhost') as client: talker = Topic(client, '/messages', 'std_msgs/String') talker.advertise() while client.is_connected: print('Sending...') talker.publish({'data': 'Hello'}) time.sleep(1) talker.unadvertise()
def _collision_object(self, collision_object, operation=CollisionObject.ADD): collision_object.operation = operation topic = Topic(self, '/collision_object', 'moveit_msgs/CollisionObject') topic.publish(collision_object.msg)
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()
import time from roslibpy import Topic from compas_fab.backends import RosClient count = 0 with RosClient('localhost') as client: talker = Topic(client, '/messages', 'std_msgs/String') talker.advertise() while client.is_connected: count += 1 msg = 'Hello #{}'.format(count) talker.publish({'data': msg}) print('Sent: ' + msg) time.sleep(1) talker.unadvertise()