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)
