def _add_payload_to_planning_scene_msg(self, payload):
msg=payload.payload_msg
co = CollisionObject()
co.id = payload.payload_msg.name
co.header.frame_id = msg.header.frame_id
payload.attached_link=msg.header.frame_id
urdf_root=self.urdf.get_root()
urdf_chain=self.urdf.get_chain(urdf_root, payload.attached_link, joints=True, links=False)
urdf_chain.reverse()
touch_links=[]
touch_root = None
for j_name in urdf_chain:
j=self.urdf.joint_map[j_name]
if j.type != "fixed":
break
touch_root=j.parent
def _touch_recurse(touch):
ret=[touch]
if touch in self.urdf.child_map:
for c_j,c in self.urdf.child_map[touch]:
if self.urdf.joint_map[c_j].type == "fixed":
ret.extend(_touch_recurse(c))
return ret
if touch_root is not None:
touch_links.extend(_touch_recurse(touch_root))
for i in xrange(len(msg.collision_geometry.mesh_resources)):
mesh_filename=urlparse.urlparse(resource_retriever.get_filename(msg.collision_geometry.mesh_resources[i])).path
mesh_pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.collision_geometry.mesh_poses[i]))
mesh_scale=msg.collision_geometry.mesh_scales[i]
mesh_scale = (mesh_scale.x, mesh_scale.y, mesh_scale.z)
mesh_msg = load_mesh_file_to_mesh_msg(mesh_filename, mesh_scale)
co.meshes.extend(mesh_msg)
co.mesh_poses.extend([mesh_pose]*len(mesh_msg))
for i in xrange(len(msg.collision_geometry.primitives)):
co.primitives.append(msg.collision_geometry.primitives[i])
primitive_pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.collision_geometry.primitive_poses[i]))
co.primitive_poses.append(primitive_pose)
aco = AttachedCollisionObject()
aco.link_name = payload.attached_link
aco.object = co
aco.touch_links = touch_links
aco.weight = msg.inertia.m
#self._pub_aco.publish(aco)
return aco
def set_goal(self, params_msg):
self.desired_transform=rox_msg.msg2transform(params_msg.desired_transform)
self.markers = self.get_all_payload_markers()
self.fixed_marker = self.markers[self.desired_transform.parent_frame_id]
self.payload_marker = self.markers[self.desired_transform.child_frame_id]
self.aruco_dict = self.get_aruco_dictionary(self.fixed_marker)
self.stage1_tol_p = params_msg.stage1_tol_p
self.stage1_tol_r = params_msg.stage1_tol_r
self.stage2_tol_p = params_msg.stage2_tol_p
self.stage2_tol_r = params_msg.stage2_tol_r
self.stage3_tol_p = params_msg.stage3_tol_p
self.stage3_tol_r = params_msg.stage3_tol_r
self.stage1_kp = params_msg.stage1_kp
self.stage2_kp = params_msg.stage2_kp
self.stage3_kp = params_msg.stage3_kp
self.K_pbvs=self.stage3_kp
self.stage2_z_offset = params_msg.stage2_z_offset
self.force_ki = params_msg.force_ki
def unpack_wrench(w):
return np.array([w.torque.x, w.torque.y, w.torque.z, w.force.x, w.force.y, w.force.z])
self.abort_force = unpack_wrench(params_msg.abort_force)
self.placement_force = unpack_wrench(params_msg.placement_force)
self._aborted=False
def get_object_pose(self, key):
self.client.wait_for_server()
goal = ObjectRecognitionGoal(False, [-1e10, 1e10, -1e10, 1e10])
self.client.send_goal(goal)
self.client.wait_for_result()
ret = self.client.get_result()
for r in ret.recognized_objects.objects:
if r.type.key == key:
return rox_msg.msg2transform(r.pose.pose.pose)
raise Exception("Requested object not found")
def compute_ik(self, pose, current_joint = None):
if isinstance(pose, PoseStamped):
pose=rox_msg.msg2transform(pose.pose)
elif isinstance(pose, Pose):
pose=rox_msg.msg2transform(pose)
joint_targets=rox.robot6_sphericalwrist_invkin(self.rox_robot, pose)
if current_joint is None:
current_joint=self.moveit_group.get_current_joint_values()
joint_target=None
d_max=1e10
for j in joint_targets:
d=np.linalg.norm(j-current_joint)
if d < d_max:
d_max = d
joint_target=np.copy(j)
if (joint_target is None):
raise Exception("Could not find target joint values")
return joint_target
def main():
rospy.init_node("pbvs_object_placement")
urdf_xml_string = rospy.get_param("robot_description")
srdf_xml_string = rospy.get_param("robot_description_semantic")
controller_commander = ControllerCommander()
#planner = Planner(controller_commander, urdf_xml_string, srdf_xml_string)
transform_fname = sys.argv[1]
camera_image_topic = sys.argv[2]
camera_trigger_topic = sys.argv[3]
camera_info_topic = sys.argv[4]
tf_listener = PayloadTransformListener()
desired_transform_msg = TransformStamped()
with open(transform_fname, 'r') as f:
transform_yaml = yaml.load(f)
genpy.message.fill_message_args(desired_transform_msg, transform_yaml)
desired_transform = rox_msg.msg2transform(desired_transform_msg)
markers = get_all_payload_markers()
fixed_marker = markers[desired_transform.parent_frame_id]
payload_marker = markers[desired_transform.child_frame_id]
aruco_dict = get_aruco_dictionary(fixed_marker)
camera_info = get_camera_info(camera_info_topic)
time.sleep(1)
dx = np.array([10000] * 6)
while True:
img = receive_image(camera_image_topic, camera_trigger_topic)
target_pose = compute_step_gripper_target_pose(img, fixed_marker, payload_marker, desired_transform, \
camera_info, aruco_dict, np.array([0.2]*6), tf_listener)
plan = planner.trajopt_plan(target_pose,
json_config_name="panel_pickup")
controller_commander.set_controller_mode(
controller_commander.MODE_HALT, 1, [], [])
controller_commander.set_controller_mode(
controller_commander.MODE_AUTO_TRAJECTORY, 1, [], [])
controller_commander.execute_trajectory(plan)
def _vision_get_object_pose(self, key):
self.overhead_vision_client.wait_for_server()
goal = ObjectRecognitionGoal(False, [-1e10, 1e10, -1e10, 1e10])
self.overhead_vision_client.send_goal(goal)
self.overhead_vision_client.wait_for_result()
ret = self.overhead_vision_client.get_result()
for r in ret.recognized_objects.objects:
if r.type.key == key:
rox_pose = rox_msg.msg2transform(r.pose.pose.pose)
rox_pose.parent_frame_id = r.pose.header.frame_id
rox_pose.child_frame_id = key
return rox_pose
raise Exception("Requested object not found")
def urdf_to_payload(xml_str):
urdf_robot = URDF.from_xml_string(xml_str)
urdf_et = ET.fromstring(xml_str)
base_links = [
u for u in urdf_robot.child_map.keys()
if u not in urdf_robot.parent_map
]
assert len(base_links) == 1, "Multiple payloads detected, invalid URDF."
base_link = base_links[0]
assert base_link not in urdf_robot.link_map, "Invalid initial_pose parent link in payload URDF"
assert len(urdf_robot.child_map[base_link]
) == 1, "Invalid initial_pose in payload URDF"
(initial_pose_joint_name,
payload_link_name) = urdf_robot.child_map[base_link][0]
assert initial_pose_joint_name.endswith(
'_initial_pose'), "Invalid initial_pose parent link in payload URDF"
#assert all([j.type == "fixed" for _, j in urdf_robot.joint_map.items()]), "All joints must be fixed type in payload URDF"
assert all([urdf_robot.parent_map[l][1] == payload_link_name for l in urdf_robot.link_map if l != payload_link_name]), \
"All links must have payload link as parent"
payload_link = urdf_robot.link_map[payload_link_name]
initial_pose_joint = urdf_robot.joint_map[initial_pose_joint_name]
payload_msg = Payload()
payload_msg.name = payload_link_name
#Load in initial pose
payload_msg.header.frame_id = initial_pose_joint.parent
payload_msg.pose = _origin_to_pose(initial_pose_joint.origin)
#Load in inertia
if payload_link.inertial is not None:
m = Inertia()
m.m = payload_link.inertial.mass
m.ixx = payload_link.inertial.inertia.ixx
m.ixy = payload_link.inertial.inertia.ixy
m.ixz = payload_link.inertial.inertia.ixz
m.iyy = payload_link.inertial.inertia.iyy
m.iyz = payload_link.inertial.inertia.iyz
m.izz = payload_link.inertial.inertia.izz
if payload_link.inertial.origin is not None:
if payload_link.inertial.origin.xyz is not None:
m.com.x = payload_link.inertial.origin.xyz[0]
m.com.y = payload_link.inertial.origin.xyz[1]
m.com.z = payload_link.inertial.origin.xyz[2]
if payload_link.inertial.origin.rpy is not None:
R = np.matrix(
rox.rpy2R(np.array(payload_link.inertial.origin.rpy)))
I = np.matrix([[m.ixx, m.ixy, m.ixz], [m.ixy, m.iyy, m.iyz],
[m.ixz, m.iyz, m.izz]])
I2 = np.dot(np.transpose(R), I).dot(R)
m.ixx = I2[0, 0]
m.ixy = I2[0, 1]
m.ixz = I2[0, 2]
m.ixy = I2[1, 0]
m.iyy = I2[1, 1]
m.iyz = I2[1, 2]
m.izz = I2[2, 2]
payload_msg.inertia = m
#Load in gripper targets
for _, l in urdf_robot.link_map.items():
m = re.match(r'^\w+_gripper_target(?:_(\d+))?$', l.name)
if m is None:
continue
j = urdf_robot.joint_map[urdf_robot.parent_map[l.name][0]]
assert j.parent == payload_link_name, "Invalid gripper_target for payload in URDF"
pose = _origin_to_pose(j.origin)
gripper_target = GripperTarget()
gripper_target.header.frame_id = payload_link_name
gripper_target.name = l.name
gripper_target.pose = pose
link_et = urdf_et.find('.//link[@name="' + l.name + '"]')
ft_et = link_et.find('.//gripper_ft_threshold')
if ft_et is not None:
pickup_et = ft_et.find('.//pickup')
if pickup_et is not None:
ft_val = [float(d) for d in pickup_et.attrib['ft'].split()]
assert len(ft_val) == 6, "Invalid pickup ft"
gripper_target.pickup_ft_threshold = ft_val
place_et = ft_et.find('.//place')
if place_et is not None:
ft_val = [float(d) for d in place_et.attrib['ft'].split()]
assert len(ft_val) == 6, "Invalid pickup ft"
gripper_target.place_ft_threshold = ft_val
payload_msg.gripper_targets.append(gripper_target)
#Load in markers
for _, l in urdf_robot.link_map.items():
m = re.match(r'^\w+_marker(?:_(\d+))?$', l.name)
if m is None:
continue
j = urdf_robot.joint_map[urdf_robot.parent_map[l.name][0]]
assert j.parent == payload_link_name, "Invalid marker for payload in URDF"
pose = _origin_to_pose(j.origin)
aruco_marker = next((x for x in payload_msg.markers if x.name == l),
None)
aruco_marker = ArucoGridboardWithPose()
aruco_marker.header.frame_id = payload_link_name
aruco_marker.name = l.name
aruco_marker.pose = pose
link_et = urdf_et.find('.//link[@name="' + l.name + '"]')
marker_et = link_et.find('.//aruco_marker')
if marker_et is not None:
gridboard_et = marker_et.find('.//gridboard')
if gridboard_et is not None:
a = gridboard_et.attrib
aruco_marker.marker.markersX = int(a['markersX'])
aruco_marker.marker.markersY = int(a['markersY'])
aruco_marker.marker.markerLength = float(a['markerLength'])
aruco_marker.marker.markerSpacing = float(a['markerSpacing'])
aruco_marker.marker.dictionary = a['dictionary']
aruco_marker.marker.firstMarker = int(a['firstMarker'])
payload_msg.markers.append(aruco_marker)
#Load in meshes
payload_geometry_chain = [(None, payload_link_name)]
if payload_link_name in urdf_robot.child_map:
payload_geometry_chain.extend(urdf_robot.child_map[payload_link_name])
for j_name, l_name in payload_geometry_chain:
#v=urdf_robot.link_map[l_name].visual
j = urdf_robot.joint_map[j_name] if j_name is not None else None
link_et = urdf_et.find('.//link[@name="' + l_name + '"]')
#Workaround to handle multiple visual elements
visual_et_s = link_et.findall('.//visual')
for visual_et in visual_et_s:
v = Visual.from_xml(visual_et)
if j is not None:
visual_pose = rox_msg.transform2pose_msg(
rox_msg.msg2transform(_origin_to_pose(j.origin)) *
rox_msg.msg2transform(_origin_to_pose(v.origin)))
else:
visual_pose = _origin_to_pose(v.origin)
if v.material is None or v.material.color is None:
visual_color = ColorRGBA(0.5, 0.5, 0.5, 1)
else:
visual_color = ColorRGBA(*v.material.color.rgba)
if isinstance(v.geometry, Mesh):
if v.geometry.scale is None:
mesh_scale = Vector3(1, 1, 1)
else:
mesh_scale = Vector3(*v.geometry.scale)
mesh_fname = v.geometry.filename
payload_msg.visual_geometry.mesh_poses.append(visual_pose)
payload_msg.visual_geometry.mesh_resources.append(mesh_fname)
payload_msg.visual_geometry.mesh_scales.append(mesh_scale)
payload_msg.visual_geometry.mesh_colors.append(visual_color)
elif isinstance(v.geometry, Box):
shape = SolidPrimitive()
shape.type = SolidPrimitive.BOX
shape.dimensions = v.geometry.size
payload_msg.visual_geometry.primitives.append(shape)
payload_msg.visual_geometry.primitive_poses.append(visual_pose)
payload_msg.visual_geometry.primitive_colors.append(
visual_color)
elif isinstance(v.geometry, Cylinder):
shape = SolidPrimitive()
shape.type = SolidPrimitive.CYLINDER
shape.dimensions = [v.geometry.length, v.geometry.radius]
payload_msg.visual_geometry.primitives.append(shape)
payload_msg.visual_geometry.primitive_poses.append(visual_pose)
payload_msg.visual_geometry.primitive_colors.append(
visual_color)
elif isinstance(v.geometry, Sphere):
shape = SolidPrimitive()
shape.type = SolidPrimitive.SPHERE
shape.dimensions = [v.geometry.radius]
payload_msg.visual_geometry.primitives.append(shape)
payload_msg.visual_geometry.primitive_poses.append(visual_pose)
payload_msg.visual_geometry.primitive_colors.append(
visual_color)
else:
assert False, "Invalid geometry in URDF"
#Workaround to handle multiple collision elements
collision_et_s = link_et.findall('.//collision')
for collision_et in collision_et_s:
v = Collision.from_xml(collision_et)
if j is not None:
collision_pose = rox_msg.transform2pose_msg(
rox_msg.msg2transform(_origin_to_pose(j.origin)) *
rox_msg.msg2transform(_origin_to_pose(v.origin)))
else:
collision_pose = _origin_to_pose(v.origin)
if isinstance(v.geometry, Mesh):
if v.geometry.scale is None:
mesh_scale = Vector3(1, 1, 1)
else:
mesh_scale = Vector3(*v.geometry.scale)
mesh_fname = v.geometry.filename
payload_msg.collision_geometry.mesh_poses.append(
collision_pose)
payload_msg.collision_geometry.mesh_resources.append(
mesh_fname)
payload_msg.collision_geometry.mesh_scales.append(mesh_scale)
elif isinstance(v.geometry, Box):
shape = SolidPrimitive()
shape.type = SolidPrimitive.BOX
shape.dimensions = v.geometry.size
payload_msg.collision_geometry.primitives.append(shape)
payload_msg.collision_geometry.primitive_poses.append(
collision_pose)
elif isinstance(v.geometry, Cylinder):
shape = SolidPrimitive()
shape.type = SolidPrimitive.CYLINDER
shape.dimensions = [v.geometry.length, v.geometry.radius]
payload_msg.collision_geometry.primitives.append(shape)
payload_msg.collision_geometry.primitive_poses.append(
collision_pose)
elif isinstance(v.geometry, Sphere):
shape = SolidPrimitive()
shape.type = SolidPrimitive.SPHERE
shape.dimensions = [v.geometry.radius]
payload_msg.collision_geometry.primitives.append(shape)
payload_msg.collision_geometry.primitive_poses.append(
collision_pose)
else:
assert False, "Invalid geometry in URDF"
payload_msg.confidence = 0.1
#TODO: read inertial values
#Sanity check
payload_msg._check_types()
return payload_msg
def runTest(self):
#msg2q, q2msg
q = np.random.rand(4)
q = q / np.linalg.norm(q)
q_msg = rox_msg.q2msg(q)
q_msg._check_types()
np.testing.assert_allclose(q, [q_msg.w, q_msg.x, q_msg.y, q_msg.z],
atol=1e-4)
q2 = rox_msg.msg2q(q_msg)
np.testing.assert_allclose(q, q2, atol=1e-4)
#msg2R, R2msg
R = rox.q2R(q)
q_msg_R = rox_msg.R2msg(R)
q_msg_R._check_types()
np.testing.assert_allclose(
q, [q_msg_R.w, q_msg_R.x, q_msg_R.y, q_msg_R.z], atol=1e-4)
R2 = rox_msg.msg2R(q_msg_R)
np.testing.assert_allclose(R, R2, atol=1e-4)
#msg2p, p2msg
p = np.random.rand(3)
p_msg = rox_msg.p2msg(p)
p_msg._check_types()
np.testing.assert_allclose(p, [p_msg.x, p_msg.y, p_msg.z], atol=1e-4)
p2 = rox_msg.msg2p(p_msg)
np.testing.assert_allclose(p, p2, atol=1e-4)
#transform messages of varying types
tf = rox.Transform(R, p)
pose_msg = rox_msg.transform2pose_msg(tf)
pose_msg._check_types()
np.testing.assert_allclose(R,
rox_msg.msg2R(pose_msg.orientation),
atol=1e-4)
np.testing.assert_allclose(p,
rox_msg.msg2p(pose_msg.position),
atol=1e-4)
pose2 = rox_msg.msg2transform(pose_msg)
np.testing.assert_allclose(R, pose2.R, atol=1e-4)
np.testing.assert_allclose(p, pose2.p, atol=1e-4)
tf_msg = rox_msg.transform2msg(tf)
tf_msg._check_types()
np.testing.assert_allclose(R,
rox_msg.msg2R(tf_msg.rotation),
atol=1e-4)
np.testing.assert_allclose(p,
rox_msg.msg2p(tf_msg.translation),
atol=1e-4)
tf2 = rox_msg.msg2transform(tf_msg)
np.testing.assert_allclose(R, tf2.R, atol=1e-4)
np.testing.assert_allclose(p, tf2.p, atol=1e-4)
#transform stamped messages of varying types
tf3 = rox.Transform(R, p, 'parent_link', 'child_link')
print tf3
print str(tf3)
pose_stamped_msg = rox_msg.transform2pose_stamped_msg(tf3)
pose_stamped_msg._check_types()
np.testing.assert_allclose(R,
rox_msg.msg2R(
pose_stamped_msg.pose.orientation),
atol=1e-4)
np.testing.assert_allclose(p,
rox_msg.msg2p(
pose_stamped_msg.pose.position),
atol=1e-4)
assert pose_stamped_msg.header.frame_id == 'parent_link'
pose3 = rox_msg.msg2transform(pose_stamped_msg)
np.testing.assert_allclose(R, pose3.R, atol=1e-4)
np.testing.assert_allclose(p, pose3.p, atol=1e-4)
assert pose3.parent_frame_id == 'parent_link'
tf_stamped_msg = rox_msg.transform2transform_stamped_msg(tf3)
tf_stamped_msg._check_types()
np.testing.assert_allclose(R,
rox_msg.msg2R(
tf_stamped_msg.transform.rotation),
atol=1e-4)
np.testing.assert_allclose(p,
rox_msg.msg2p(
tf_stamped_msg.transform.translation),
atol=1e-4)
assert tf_stamped_msg.header.frame_id == 'parent_link'
assert tf_stamped_msg.child_frame_id == 'child_link'
tf4 = rox_msg.msg2transform(tf_stamped_msg)
np.testing.assert_allclose(R, tf4.R, atol=1e-4)
np.testing.assert_allclose(p, tf4.p, atol=1e-4)
assert tf4.parent_frame_id == 'parent_link'
assert tf4.child_frame_id == 'child_link'
#msg2twist, twist2msg
twist = np.random.rand(6)
twist_msg = rox_msg.twist2msg(twist)
twist_msg._check_types()
np.testing.assert_allclose(twist, [twist_msg.angular.x, twist_msg.angular.y, twist_msg.angular.z, \
twist_msg.linear.x, twist_msg.linear.y, twist_msg.linear.z], \
atol=1e-4)
twist2 = rox_msg.msg2twist(twist_msg)
np.testing.assert_allclose(twist, twist2, atol=1e-4)
#msg2wrench, wrench2msg
wrench = np.random.rand(6)
wrench_msg = rox_msg.wrench2msg(wrench)
wrench_msg._check_types()
np.testing.assert_allclose(wrench, [wrench_msg.torque.x, wrench_msg.torque.y, wrench_msg.torque.z, \
wrench_msg.force.x, wrench_msg.force.y, wrench_msg.force.z], \
atol=1e-4)
wrench2 = rox_msg.msg2wrench(wrench_msg)
np.testing.assert_allclose(wrench, wrench2, atol=1e-4)
def _publish_rviz_sim_cameras(self):
marker_array=MarkerArray()
for p in self.payloads:
payload=self.payloads[p]
msg=payload.payload_msg
id = 0
for i in xrange(len(msg.visual_geometry.mesh_resources)):
marker=Marker()
marker.ns="payload_" + msg.name
marker.id=id
marker.type=marker.MESH_RESOURCE
marker.action=marker.ADD
marker.mesh_resource=msg.visual_geometry.mesh_resources[i]
marker.mesh_use_embedded_materials=True
marker.pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.visual_geometry.mesh_poses[i]))
marker.header.frame_id = payload.attached_link
marker.scale=msg.visual_geometry.mesh_scales[i]
if i < len(msg.visual_geometry.mesh_colors):
marker.color=msg.visual_geometry.mesh_colors[i]
else:
marker.color.a=1
marker.color.r=0.5
marker.color.g=0.5
marker.color.b=0.5
marker.header.stamp=rospy.Time.now()
marker.frame_locked=True
marker._check_types()
#marker.colors.append(ColorRGBA(0.5,0.5,0.5,1))
marker_array.markers.append(marker)
id += 1
for i in xrange(len(msg.visual_geometry.primitives)):
marker=Marker()
marker.ns="payload_" + msg.name
marker.id=id
primitive = msg.visual_geometry.primitives[i]
primitive_type = primitive.type
if primitive_type == SolidPrimitive.BOX:
marker.type=Marker.CUBE
assert len(primitive.dimensions) == 3
marker.scale = Vector3(*primitive.dimensions)
elif primitive_type == SolidPrimitive.CYLINDER:
marker.type=Marker.CYLINDER
assert len(primitive.dimensions) == 2
marker.scale = Vector3(primitive.dimensions[1]*2, primitive.dimensions[1]*2, primitive.dimensions[0])
elif primitive_type == SolidPrimitive.SPHERE:
marker.type=Marker.SPHERE
assert len(primitive.dimensions) == 1
marker.scale = Vector3(*([primitive.dimensions[0]*2]*3))
else:
assert False, "Invalid geometry specified for SolidPrimitive"
marker.action=marker.ADD
marker.pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.visual_geometry.primitive_poses[i]))
marker.header.frame_id = payload.attached_link
if i < len(msg.visual_geometry.primitive_colors):
marker.color=msg.visual_geometry.primitive_colors[i]
else:
marker.color.a=1
marker.color.r=0.5
marker.color.g=0.5
marker.color.b=0.5
marker.header.stamp=rospy.Time.now()
marker.frame_locked=True
marker._check_types()
marker_array.markers.append(marker)
id += 1
marker_array._check_types
self.rviz_cam_publisher.publish(marker_array)
def execute_overhead_callback(self, goal):
now = rospy.Time.now()
if self.ros_overhead_cam_info is None:
raise Exception("Camera Info not received")
self.last_ros_image_stamp = self.ros_image_stamp
try:
self.ros_overhead_trigger.wait_for_service(timeout=0.1)
self.ros_overhead_trigger()
except:
pass
wait_count = 0
while self.ros_overhead_cam_info.ros_image is None or self.ros_image_stamp == self.last_ros_image_stamp:
if wait_count > 250:
raise Exception("Image receive timeout")
time.sleep(0.25)
wait_count += 1
print "Past timeout"
#stored data in local function variable, so changed all references off of self.ros_image
img = self.ros_overhead_cam_info.ros_image
if img is None:
raise Exception("Camera image data not received")
print "image received"
r_array = RecognizedObjectArray()
r_array.header.stamp = now
r_array.header.frame_id = self.frame_id
if goal.use_roi:
raise Warning("use_roi in ObjectRecognitionRequest ignored")
search_objects = dict()
with self.payloads_lock:
for _, p in self.payloads.items():
search_objects[p.name] = p.markers
for _, l in self.link_markers.items():
search_objects[l.header.frame_id] = l.markers
#TODO: handle multiple aruco dictionaries, currently only use one
print "past payload lock"
aruco_dicts = set()
for m in search_objects.itervalues():
for m2 in m:
aruco_dicts.add(m2.marker.dictionary)
print len(aruco_dicts)
aruco_dicts.add("DICT_ARUCO_ORIGINAL")
assert len(
aruco_dicts
) == 1, "Currently all tags must be from the same dictionary"
if not hasattr(cv2.aruco, next(iter(aruco_dicts))):
raise ValueError("Invalid aruco-dict value")
aruco_dict_id = getattr(cv2.aruco, next(iter(aruco_dicts)))
aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_id)
c_pose = self.listener.lookupTransform(
"/world", self.ros_overhead_cam_info.ros_data.header.frame_id,
rospy.Time(0))
print "c_pose"
print c_pose.p
print c_pose.R
parameters = cv2.aruco.DetectorParameters_create()
parameters.cornerRefinementWinSize = 32
parameters.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
img, aruco_dict, parameters=parameters)
print ids
for object_name, payload_markers in search_objects.items():
tag_object_poses = dict()
for m in payload_markers:
board = get_aruco_gridboard(m.marker, aruco_dict)
#board = cv2.aruco.GridBoard_create(4, 4, .04, .0075, aruco_dict, 16)
retval, rvec, tvec = cv2.aruco.estimatePoseBoard(
corners, ids, board, self.ros_overhead_cam_info.camMatrix,
self.ros_overhead_cam_info.distCoeffs)
print retval
print rvec
print tvec
if (retval > 0):
if (m.name == "leeward_mid_panel_marker_1"
or m.name == "leeward_tip_panel_marker_1"):
print "Found tag: " + m.name
try:
#o_pose = self.listener.lookupTransform(m.name, object_name, rospy.Time(0))
o_pose = rox_msg.msg2transform(m.pose).inv()
print object_name
print o_pose
print ""
print "o_pose"
print o_pose.p
print o_pose.R
Ra, b = cv2.Rodrigues(rvec)
a_pose = rox.Transform(Ra, tvec)
print "a_pose"
print a_pose.p
print a_pose.R
tag_object_poses[m.name] = c_pose * (a_pose *
o_pose)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
#TODO: merge tag data if multiple tags detected
if len(tag_object_poses) > 0:
p = PoseWithCovarianceStamped()
p.pose.pose = rox_msg.transform2pose_msg(
tag_object_poses.itervalues().next())
p.header.stamp = now
p.header.frame_id = self.frame_id
r = RecognizedObject()
r.header.stamp = now
r.header.frame_id = self.frame_id
r.type.key = object_name
r.confidence = 1
r.pose = p
r_array.objects.append(r)
"""for o in self.object_names:
try:
(trans,rot) = self.listener.lookupTransform("/world", o, rospy.Time(0))
print o
print trans
print rot
print ""
p=PoseWithCovarianceStamped()
p.pose.pose=Pose(Point(trans[0], trans[1], trans[2]), Quaternion(rot[0], rot[1], rot[2], rot[3]))
p.header.stamp=now
p.header.frame_id=self.frame_id
r=RecognizedObject()
r.header.stamp=now
r.header.frame_id=self.frame_id
r.type.key=o
r.confidence=1
r.pose=p
r_array.objects.append(r)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue"""
print "Succeeded in finding tags"
result = ObjectRecognitionResult()
result.recognized_objects = r_array
self.overhead_server.set_succeeded(result=result)