def test_attached_collision_object(self):
expected_scene_1 = rospy.wait_for_message("/expected_planning_scene_1",
PlanningScene,
timeout=10)
scene_1 = rospy.wait_for_message('/planning_scene',
PlanningScene,
timeout=10)
self._assert_planning_scene_equal(expected_scene_1, scene_1)
rospy.sleep(rospy.Duration(1))
update_pose_proxy = rospy.ServiceProxy("update_payload_pose",
UpdatePayloadPose)
payload2_to_gripper_target_tf = self.tf_listener.lookupTransform(
"payload2", "payload2_gripper_target", rospy.Time(0))
req = UpdatePayloadPoseRequest()
req.header.frame_id = "vacuum_gripper_tool"
req.name = "payload2"
req.pose = rox_msg.transform2pose_msg(
payload2_to_gripper_target_tf.inv())
req.confidence = 0.5
res = update_pose_proxy(req)
assert res.success
expected_scene_2 = rospy.wait_for_message("/expected_planning_scene_2",
PlanningScene,
timeout=10)
scene_2 = rospy.wait_for_message('/planning_scene',
PlanningScene,
timeout=10)
self._assert_planning_scene_equal(expected_scene_2, scene_2)
world_to_fixture2_payload2_target_tf = self.tf_listener.lookupTransform(
"world", "fixture2_payload2_target", rospy.Time(0))
#Add in an offset to represent a final placement error
fixture2_payload2_target_to_payload2_tf = rox.Transform(
rox.rot([0, 0, 1], np.deg2rad(5)), [0.1, 0, 0],
"fixture2_payload2_target", "payload2")
req2 = UpdatePayloadPoseRequest()
req2.header.frame_id = "world"
req2.name = "payload2"
req2.pose = rox_msg.transform2pose_msg(
world_to_fixture2_payload2_target_tf *
fixture2_payload2_target_to_payload2_tf)
res2 = update_pose_proxy(req2)
assert res2.success
expected_scene_3 = rospy.wait_for_message("/expected_planning_scene_3",
PlanningScene,
timeout=10)
scene_3 = rospy.wait_for_message('/planning_scene',
PlanningScene,
timeout=10)
self._assert_planning_scene_equal(expected_scene_3, scene_3)
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 _update_payload_pose(self,
payload_name,
pose,
parent_frame_id=None,
confidence=0.1):
payload = self._get_payload(payload_name)
if parent_frame_id is None:
parent_frame_id = payload.header.frame_id
parent_tf = self.tf_listener.lookupTransform(parent_frame_id,
pose.parent_frame_id,
rospy.Time(0))
pose2 = parent_tf.inv() * pose
req = UpdatePayloadPoseRequest()
req.name = payload_name
req.pose = rox_msg.transform2pose_msg(pose2)
req.header.frame_id = parent_frame_id
req.confidence = confidence
res = self.update_payload_pose_srv(req)
if not res.success:
raise Exception("Could not update payload pose")
def compute_cartesian_path(self, pose_target, jump_threshold=0.01, eef_step=0.0, avoid_collisions=True):
if isinstance(pose_target, PoseStamped):
pose_target=pose_target.pose
elif isinstance(pose_target,rox.Transform):
pose_target=rox_msg.transform2pose_msg(pose_target)
(path, fraction) = self.moveit_group.compute_cartesian_path([pose_target], jump_threshold,\
eef_step, avoid_collisions)
if (fraction < 0.9999):
raise Exception("Could not compute cartesian path")
return path
def _update_payload_pose(self, payload_name, pose, parent_frame_id = None, confidence = 0.1):
with self.payloads_lock:
n=rospy.Time.now()
payload=copy.deepcopy(self.payloads[payload_name].payload_msg)
if parent_frame_id is None:
parent_frame_id = payload.header.frame_id
parent_tf = self.tf_listener.lookupTransform(parent_frame_id, pose.parent_frame_id, rospy.Time(0))
pose2=parent_tf.inv() * pose
payload.pose=rox_msg.transform2pose_msg(pose2)
payload.header.frame_id=parent_frame_id
payload.header.stamp=n
payload.confidence = confidence
payload_a=PayloadArray()
payload_a.payloads.append(payload)
payload_a.header.stamp=n
self._payload_msg_pub.publish(payload_a)
def execute_callback(self, goal):
now = rospy.Time.now()
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")
for o in self.object_names:
try:
object_tf = self.listener.lookupTransform(
"/world", o, rospy.Time(0))
print o
print object_tf
print ""
p = PoseWithCovarianceStamped()
p.pose.pose = rox_msg.transform2pose_msg(object_tf)
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
result = ObjectRecognitionResult()
result.recognized_objects = r_array
self.server.set_succeeded(result=result)
def trajopt_plan(self,
target_pose,
json_config_str=None,
json_config_name=None,
target_joints=None):
with self.lock:
if (json_config_str is None and json_config_name is not None):
json_config_str = self.load_json_config(json_config_name)
robot = self.controller_commander.rox_robot
#vel_upper_lim = np.array(robot.joint_vel_limit) * speed_scalar
#vel_lower_lim = -vel_upper_lim
#joint_lower_limit = np.array(robot.joint_lower_limit)
#joint_upper_limit = np.array(robot.joint_upper_limit)
joint_names = robot.joint_names
joint_positions = self.controller_commander.get_current_joint_values(
)
if target_pose is not None:
p = PoseArray()
p.header.frame_id = "world"
p.header.stamp = rospy.Time.now()
p.poses.append(
rox_msg.transform2pose_msg(
self.controller_commander.compute_fk(joint_positions)))
p.poses.append(rox_msg.transform2pose_msg(target_pose))
self.waypoint_plotter.publish(p)
self.tesseract_env.setState(joint_names, joint_positions)
init_pos = self.tesseract_env.getCurrentJointValues()
self.tesseract_plotter.plotTrajectory(
self.tesseract_env.getJointNames(),
np.reshape(init_pos, (1, 6)))
planner = tesseract.TrajOptPlanner()
manip = "move_group"
end_effector = "vacuum_gripper_tool"
pci = tesseract.ProblemConstructionInfo(self.tesseract_env)
pci.fromJson(json_config_str)
pci.kin = self.tesseract_env.getManipulator(manip)
pci.init_info.type = tesseract.InitInfo.STATIONARY
#pci.init_info.dt=0.5
if target_pose is not None:
#Final target_pose constraint
pose_constraint = tesseract.CartPoseTermInfo()
pose_constraint.term_type = tesseract.TT_CNT
pose_constraint.link = end_effector
pose_constraint.timestep = pci.basic_info.n_steps - 1
q = rox.R2q(target_pose.R)
pose_constraint.wxyz = np.array(q)
pose_constraint.xyz = np.array(target_pose.p)
pose_constraint.pos_coefs = np.array(
[1000000, 1000000, 1000000], dtype=np.float64)
pose_constraint.rot_coefs = np.array([10000, 10000, 10000],
dtype=np.float64)
pose_constraint.name = "final_pose"
pci.cnt_infos.push_back(pose_constraint)
elif target_joints is not None:
joint_constraint = tesseract.JointPosTermInfo()
joint_constraint.term_type = tesseract.TT_CNT
joint_constraint.link = end_effector
joint_constraint.first_step = pci.basic_info.n_steps - 2
joint_constraint.last_step = pci.basic_info.n_steps - 1
#joint_constraint.coeffs = tesseract.DblVec([10000]*6)
joint_constraint.targets = tesseract.DblVec(
list(target_joints))
print target_joints
pci.cnt_infos.push_back(joint_constraint)
else:
assert False
prob = tesseract.ConstructProblem(pci)
config = tesseract.TrajOptPlannerConfig(prob)
config.params.max_iter = 1000
planning_response = planner.solve(config)
if (planning_response.status_code != 0):
raise Exception(
"TrajOpt trajectory planning failed with code: %d" %
planning_response.status_code)
self.tesseract_plotter.plotTrajectory(
self.tesseract_env.getJointNames(),
planning_response.trajectory[:, 0:6])
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = joint_names
trajectory_time = np.cumsum(1.0 /
planning_response.trajectory[:, 6])
trajectory_time = trajectory_time - trajectory_time[0]
for i in xrange(planning_response.trajectory.shape[0]):
jt_p = JointTrajectoryPoint()
jt_p.time_from_start = rospy.Duration(trajectory_time[i])
jt_p.positions = planning_response.trajectory[i, 0:6]
jt.points.append(jt_p)
return jt
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 _origin_to_pose(origin):
R = rox.rpy2R(origin.rpy) if origin.rpy is not None else np.eye(3)
p = origin.xyz if origin.xyz is not None else np.zeros((3, ))
t = rox.Transform(R, p)
return rox_msg.transform2pose_msg(t)
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)