def sendCommandToRobot(self, data):
splitData = data.split(":")
targetRobot = splitData[0]
command = splitData[1][1:]
if targetRobot == "All":
for (robot_name, publisher) in self._robotDict.iteritems():
msg = StringMsg()
msg.data = command
publisher.publish(msg)
else:
publisher = self._robotDict[targetRobot]
msg = StringMsg()
msg.data = command
publisher.publish(msg)
def publish_updates(self):
"""Update each state machine."""
for i, publisher in enumerate(self.sm_publishers):
publisher.publish(self.state_machines[i].msg())
# Create a string containing the info of all state machines.
self.info_publisher.publish(
StringMsg("\n".join(f"{sm.__class__.__name__}: {sm.info()}"
for sm in self.state_machines)))
def _create_ros2msg(self):
r"""
Creates the ROS2 message as a lowercase alphabetical random string
with variable length. The length is selected by the user at launch time
with '-s' parameter.
"""
payload = ''.join(
[chr(np.random.randint(65, 90)) for _ in range(self._msg_size)])
return StringMsg(data=payload)
def _send_callback(self):
if self._curr_iters < self._max_iters:
msg = ''.join(
[chr(random.randint(65, 90)) for _ in range(self._msg_size)])
self.get_logger().info(
f"publishing message [{self._curr_iters+1}/{self._max_iters}]: {msg}"
)
ros2msg = StringMsg(data=msg)
self._publisher.publish(ros2msg)
self._curr_iters += 1
else:
if self._max_iters > 0:
self.get_logger().info("Exiting... max num iters reached")
else:
self.get_logger().info("Exiting... simulation time expired")
self._publisher.publish(StringMsg(data=STOP_MESSAGE))
time.sleep(0.5)
self.destroy_timer(self._pub_timer)
self.destroy_publisher(self._publisher)
self._exit_event.set_result(None)
def registerScan(self, scan_data):
# registers laser scan and publishes position of closest object (or point rather)
ranges = np.array(scan_data.ranges)
# sort by distance to check from closer to further away points if they might be something real
sortedIndices = np.argsort(ranges)
minDistanceID = None
minDistance = float('inf')
#rospy.logwarn('88')
if (not (self.lastScan is None)):
# if we already have a last scan to compare to:
for i in sortedIndices:
# check all distance measurements starting from the closest one
tempMinDistance = ranges[i]
# now we check if this might be noise:
# get a window. in it we will check if there has been a scan with similar distance
# in the last scan within that window
# we kneed to clip the window so we don't have an index out of bounds
windowIndex = np.clip([i - self.winSize, i + self.winSize + 1],
0, len(self.lastScan))
window = self.lastScan[windowIndex[0]:windowIndex[1]]
with np.errstate(invalid='ignore'):
# check if any of the scans in the window (in the last scan) has a distance close enough to the current one
if (np.any(
abs(window - tempMinDistance) <= self.deltaDist)):
# this will also be false for all tempMinDistance = NaN or inf
# we found a plausible distance
minDistanceID = i
minDistance = ranges[minDistanceID]
break # at least one point was equally close
# so we found a valid minimum and can stop the loop
self.lastScan = ranges
#catches no scan, no minimum found, minimum is actually inf
if (minDistance > scan_data.range_max):
#means we did not really find a plausible object
# publish warning that we did not find anything
rospy.logwarn('laser no object found')
self.infoPublisher.publish(StringMsg('laser:nothing found'))
else:
# calculate angle of the objects location. 0 is straight ahead
minDistanceAngle = scan_data.angle_min + minDistanceID * scan_data.angle_increment
# here we only have an x angle, so the y is set arbitrarily
self.positionPublisher.publish(
PositionMsg(minDistanceAngle, 42, minDistance))
def _activation_callback(self):
r"""
In order to wait the correct deployment of all the ROS resources invoked,
a proper 'activation_timer' is created during the init phase of the Sender node.
This method is the callback connected with the activation_timer and manages
the 'living phase' of the node
"""
# check if ROS2 executor is assigned
if self.executor is None:
self.get_logger().info(f"Node {self.get_name()} not ready, executor absent. " \
"Waiting for {self._activation_delay}.")
else:
self.get_logger().info(
f"Node {self.get_name()} ready, execution starts...")
self.destroy_timer(self._activation_timer)
self._execute()
print("Sending STOP message")
self._sys_pub.publish(StringMsg(data=Constants.Comms.STOP_MESSAGE))
print("Activation callback ENDs")
self._explicit_destroy()
def select_marker(self, marker_id):
"""Selects the marker matching the given Id.
:param marker_id: Marker to select
:type marker_id: str
"""
if self._selected_object != marker_id:
if self._selected_object in self.markers:
cm, cb = self.markers[self._selected_object]
activateMarker(cm, False)
self.marker_server.insert(cm, cb)
self._selected_object = marker_id
if self._selected_object in self.markers:
cm, cb = self.markers[self._selected_object]
activateMarker(cm, True)
self.marker_server.insert(cm, cb)
self.marker_server.applyChanges()
msg = StringMsg()
msg.data = self._selected_object if self._selected_object != None else ''
self.pub_selection.publish(msg)
#!/usr/bin/env python
import argparse
from std_msgs.msg import String as StringMsg
from boost_python_minimal._cpp_wrapper import _print_bytes
from boost_python_minimal.bytes_example import print_string_message, bytes_to_string_message
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Example usage of the minimal Boost Python library.')
parser.add_argument('test_string', help='a string to test Boost Python functions')
args = parser.parse_args()
print(f"received test string: '{args.test_string}'")
print("now printing a bytearray containing test string using exposed C++ code...")
test_bytes = bytes(args.test_string, 'utf-8')
_print_bytes(test_bytes)
print("now creating a ROS string message and print it using exposed C++ code...")
msg = StringMsg()
msg.data = args.test_string
print_string_message(msg)
print("now converting a bytearray containing test string to a ROS string message using exposed C++ code...")
msg2 = bytes_to_string_message(test_bytes)
print(f"message: type={type(msg2)}, data='{msg2.data}'")
def trackObject(self, image_data, depth_data):
if (image_data.encoding != 'rgb8'):
raise ValueError('image is not rgb8 as expected')
#convert both images to numpy arrays
frame = self.bridge.imgmsg_to_cv2(image_data, desired_encoding='rgb8')
depthFrame = self.bridge.imgmsg_to_cv2(
depth_data, desired_encoding='passthrough') #"32FC1")
if (np.shape(frame)[0:2] != (self.pictureHeight, self.pictureWidth)):
raise ValueError(
'image does not have the right shape. shape(frame): {}, shape parameters:{}'
.format(
np.shape(frame)[0:2],
(self.pictureHeight, self.pictureWidth)))
# blure a little and convert to HSV color space
blurred = cv2.GaussianBlur(frame, (11, 11), 0)
hsv = cv2.cvtColor(blurred, cv2.COLOR_RGB2HSV)
# select all the pixels that are in the range specified by the target
org_mask = cv2.inRange(hsv, self.targetUpper, self.targetLower)
# clean that up a little, the iterations are pretty much arbitrary
mask = cv2.erode(org_mask, None, iterations=4)
mask = cv2.dilate(mask, None, iterations=3)
# find contours of the object
contours = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
newPos = None #if no contour at all was found the last position will again be set to none
# lets you display the image for debuging. Not in realtime though
if displayImage:
backConverted = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)
#cv2.imshow('frame', backConverted)
#cv2.waitKey(0)
#print(backConverted)
plt.figure()
plt.subplot(2, 2, 1)
plt.imshow(frame)
plt.xticks([]), plt.yticks([])
plt.subplot(2, 2, 2)
plt.imshow(org_mask, cmap='gray', interpolation='bicubic')
plt.xticks([]), plt.yticks([])
plt.subplot(2, 2, 3)
plt.imshow(mask, cmap='gray', interpolation='bicubic')
plt.xticks([]), plt.yticks([])
plt.show()
rospy.sleep(0.2)
# go threw all the contours. starting with the bigest one
for contour in sorted(contours, key=cv2.contourArea, reverse=True):
# get position of object for this contour
pos = self.analyseContour(contour, depthFrame)
# if it's the first one we found it will be the fall back for the next scan if we don't find a plausible one
if newPos is None:
newPos = pos
# check if the position is plausible
if self.checkPosPlausible(pos):
self.lastPosition = pos
self.publishPosition(pos)
return
self.lastPosition = newPos #we didn't find a plossible last position, so we just save the biggest contour
# and publish warnings
rospy.loginfo('no position found')
self.infoPublisher.publish(StringMsg('visual:nothing found'))
def registerScan(self, scan_data):
# 获取雷达数据
ranges = np.array(scan_data.ranges)
# 跟随目标信息
goalObjectID = None
goalObjectDistance = float('inf')
goalObjectAngle = float('inf')
# 激光扫描角度值
angles = np.zeros(len(ranges))
for i in range(len(angles)):
angles[i] = scan_data.angle_min + i * scan_data.angle_increment
# 求每个点的差分值
diff = np.zeros(len(ranges))
for i in range(len(diff)):
diff[i] = ranges[i] - ranges[i - 1]
# 根据差分值，寻找物体边界
edge = np.zeros([0, 2], dtype=np.int)
for i in range(len(diff)):
if abs(diff[i]) > self.diff_max:
edge = np.append(edge, [[i - 1, i]], axis=0)
# 根据边界，计算物体宽度(余弦定理)
object_width = np.zeros(len(edge))
for i in range(len(object_width)):
object_width[i] = np.sqrt(
np.square(ranges[edge[i][0]]) +
np.square(ranges[edge[i - 1][1]]) -
2 * ranges[edge[i][0]] * ranges[edge[i - 1][1]] *
math.cos(angles[edge[i][0]] - angles[edge[i - 1][1]]))
# 寻找符合设定宽度阈值的物体ID
objectID = np.zeros(0, dtype=np.int)
for i in range(len(object_width)):
if self.object_min < object_width[i] < self.object_max:
objectID = np.append(objectID,
(edge[i][0] + edge[i - 1][1]) // 2)
# 如果objectID为空，说明没有找到符合阈值的物体，系统发布没有找到物体，停止小车运动
if len(objectID) == 0:
self.infoPublisher.publish(StringMsg('Laser not find object!!!'))
else:
# 得到符合设定宽度阈值的物体的距离信息
distance = np.zeros(len(objectID))
for i in range(len(distance)):
distance[i] = ranges[objectID[i]]
# 将距离从小到大排序
sortedIndex = np.argsort(distance)
goalObjectID = objectID[sortedIndex[0]]
goalObjectDistance = ranges[goalObjectID]
goalObjectAngle = angles[goalObjectID]
# 滤波处理（判断当前距离与上一次的距离，如果差值过大，则当前可能是噪声，将当前值改为上次的距离）
if goalObjectDistance - self.lastgoalObjectDistance > self.filter_distance:
goalObjectDistance = self.lastgoalObjectDistance
# 距离太远，系统发布物体距离过远，小车停止运动
if goalObjectDistance > self.max_tracker_distance:
self.infoPublisher.publish(StringMsg('Object is too far!!!'))
rospy.logwarn(
'goalObjectDistance: {} M'.format(goalObjectDistance))
else:
# 发布目标的角度和距离
self.positionPublisher.publish(
PositionMsg(goalObjectAngle, 0, goalObjectDistance))
# 将当前距离值赋给上一次距离值
self.lastgoalObjectDistance = goalObjectDistance
def start_planner(self, request):
rospy.loginfo('/%s/start_planner/ Generating knowledge base and starting planner' % self.node_name)
self.generate_knowledge_base()
self.cmd_pub.publish(StringMsg("plan"))
return EmptyResponse()
def run(self):
port = self._server_sock.getsockname()[1]
advertise_service( self._server_sock, "GlassServer",
service_id = self._uuid,
service_classes = [ self._uuid, SERIAL_PORT_CLASS ],
profiles = [ SERIAL_PORT_PROFILE ],
# protocols = [ OBEX_UUID ]
)
rospy.loginfo("Waiting for connection on RFCOMM channel %d" % port)
self._client_sock, self._client_info = self._server_sock.accept()
self._client_sock.setblocking(1)
rospy.loginfo("Accepted connection from " + str(self._client_info))
try:
#Make configuration service visible
self._config_srv = rospy.Service("robot_configuration", RobotConfiguration, self.robotConfig)
while not rospy.is_shutdown():
data = ""
#ready = select.select([self._client_sock], [], [], 0)
#if ready[0]:
data = self._client_sock.recv(1024)
if len(data) > 0:
rospy.loginfo("received: \"%s\"" % data)
if data == "end connection\n":
break
elif data == "Confirm connection\n":
self._lock.acquire()
self._client_sock.send("Connection confirmed\n")
self._lock.release()
elif data == "configuration complete\n":
# self._client_sock.send("Copy: %s\n" % data)
self._config_success = True
elif data == "Delivery failed":
pass
elif data == "_NEXT_\n":
self._talkback_event.set()
elif not data == "":
# rospy.loginfo("Sending copy")
# self._client_sock.send("Copy: %s" % data)
msg = StringMsg()
msg.data = data
self._voice_pub.publish(msg)
self.sendCommandToRobot(data)
#End the configuration service and close the connection
self._config_srv = None
rospy.loginfo("Disconnecting")
self._client_sock.close()
self._server_sock.close()
rospy.loginfo("all done")
except IOError, e:
rospy.loginfo("Error")
print e
def main():
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
package_map = PackageMap()
package_map.PopulateFromEnvironment('AMENT_PREFIX_PATH')
sdf_file_path = os.path.join(package_map.GetPath('iiwa14_description'),
'iiwa14_no_collision.sdf')
joint_names = [
'iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4',
'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'
]
joints = []
for name in joint_names:
joints.append(station.get_controller_plant().GetJointByName(name))
rclpy.init()
node = rclpy.create_node('interactive_demo')
# Publish SDF content on robot_description topic
latching_qos = QoSProfile(depth=1,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
description_publisher = node.create_publisher(StringMsg,
'robot_description',
qos_profile=latching_qos)
msg = StringMsg()
with open(sdf_file_path, 'r') as sdf_file:
msg.data = sdf_file.read()
description_publisher.publish(msg)
clock_system = SystemClock()
builder.AddSystem(clock_system)
# Transform broadcaster for TF frames
tf_broadcaster = TransformBroadcaster(node)
# Connect to system that publishes TF transforms
tf_system = builder.AddSystem(
TFPublisher(tf_broadcaster=tf_broadcaster, joints=joints))
tf_buffer = Buffer(node=node)
tf_listener = TransformListener(tf_buffer, node)
server = InteractiveMarkerServer(node, 'joint_targets')
joint_target_system = builder.AddSystem(
MovableJoints(server, tf_buffer, joints))
fk_system = builder.AddSystem(
ForwardKinematics(station.get_controller_plant()))
builder.Connect(station.GetOutputPort('iiwa_position_measured'),
fk_system.GetInputPort('joint_positions'))
builder.Connect(fk_system.GetOutputPort('transforms'),
tf_system.GetInputPort('body_poses'))
builder.Connect(clock_system.GetOutputPort('clock'),
tf_system.GetInputPort('clock'))
builder.Connect(joint_target_system.GetOutputPort('joint_states'),
station.GetInputPort('iiwa_position'))
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
constant_sys = builder.AddSystem(ConstantVectorSource(numpy.array([0.107
])))
builder.Connect(constant_sys.get_output_port(0),
station.GetInputPort("wsg_position"))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator_context = simulator.get_mutable_context()
simulator.set_target_realtime_rate(1.0)
station_context = station.GetMyMutableContextFromRoot(simulator_context)
num_iiwa_joints = station.num_iiwa_joints()
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, numpy.zeros(num_iiwa_joints))
while simulator_context.get_time() < 12345:
simulator.AdvanceTo(simulator_context.get_time() + 0.01)
# TODO(sloretz) really need a spin_some in rclpy
rclpy.spin_once(node, timeout_sec=0)
rclpy.shutdown()
def ping(self, msg='Python ping'):
message = StringMsg()
message.data = msg
self._publisher.publish(message)
def bytes_to_string_message(bytes_data):
msg = StringMsg()
seralizedMsgView = _bytes_to_string_message(bytes_data)
return msg.deserialize(seralizedMsgView.tobytes())
