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 RunScript(self, robot, planes, start_configuration, group,
attached_collision_meshes, path_constraints, max_step,
compute):
key = create_id(self, 'trajectory')
max_step = float(max_step) if max_step else 0.01
path_constraints = list(path_constraints) if path_constraints else None
attached_collision_meshes = list(
attached_collision_meshes) if attached_collision_meshes else None
if robot and robot.client and robot.client.is_connected and start_configuration and planes and compute:
frames = [
RhinoPlane.from_geometry(plane).to_compas_frame()
for plane in planes
]
st[key] = robot.plan_cartesian_motion(
frames,
start_configuration=start_configuration,
group=group,
options=dict(
max_step=max_step,
path_constraints=path_constraints,
attached_collision_meshes=attached_collision_meshes))
trajectory = st.get(key, None)
return trajectory
def RunScript(self, ros_client, topic_name, topic_type, interval, start,
stop):
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')
if not hasattr(self, 'message_count'):
self.message_count = 0
self.interval = interval or 25 # in milliseconds
self.is_updating = False
self.is_subscribed = False
self.msg_key = create_id(self, 'last_msg')
key = create_id(self, 'topic')
last_msg = st.get(self.msg_key, None)
topic = st.get(key, None)
if ros_client and ros_client.is_connected:
if start:
self._unsubscribe(topic)
topic = Topic(ros_client, topic_name, topic_type)
topic.subscribe(self.topic_callback)
time.sleep(0.2)
st[key] = topic
if stop:
self._unsubscribe(topic)
self.is_subscribed = topic and topic.is_subscribed
if last_msg:
last_msg = ROSmsg.parse(last_msg, topic_type)
if self.is_subscribed:
self.Message = 'Subscribed, {} messages'.format(self.message_count)
else:
self.Message = 'Not subscribed'
return (topic, last_msg, self.is_subscribed)
def RunScript(self, ros_client, load):
key = create_id(self, 'robot')
if ros_client and ros_client.is_connected and load:
# Load URDF from ROS
st[key] = ros_client.load_robot(load_geometry=True, precision='12f')
st[key].artist = RobotModelArtist(st[key].model)
robot = st.get(key, None)
if robot: # client sometimes need to be restarted, without needing to reload geometry
robot.client = ros_client
return robot
def RunScript(self, ip, port, connect):
ros_client = None
ip = ip or '127.0.0.1'
port = port or 9090
key = create_id(self, 'ros_client')
ros_client = st.get(key, None)
if ros_client:
st[key].close()
if connect:
st[key] = RosClient(ip, port)
st[key].run(5)
ros_client = st.get(key, None)
is_connected = ros_client.is_connected if ros_client else False
return (ros_client, is_connected)
def RunScript(self, robot, goal_constraints, start_configuration, group, attached_collision_meshes, path_constraints, planner_id, compute):
key = create_id(self, 'trajectory')
path_constraints = list(path_constraints) if path_constraints else None
attached_collision_meshes = list(attached_collision_meshes) if attached_collision_meshes else None
planner_id = str(planner_id) if planner_id else 'RRTConnectkConfigDefault'
if robot and robot.client and robot.client.is_connected and start_configuration and goal_constraints and compute:
st[key] = robot.plan_motion(goal_constraints,
start_configuration=start_configuration,
group=group,
options=dict(
attached_collision_meshes=attached_collision_meshes,
path_constraints=path_constraints,
planner_id=planner_id,
))
trajectory = st.get(key, None)
return trajectory
def RunScript(self, robot):
key = create_id(self, 'planning_scene')
if robot:
st[key] = PlanningScene(robot)
return st.get(key, None)
