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()
