class WorldManagerServer:
def __init__(self):
rospy.init_node('world_manager_node')
moveit_commander.roscpp_initialize(sys.argv)
# wait for moveit to come up
self.scene = PlanningSceneInterface()
self.tf_manager = TFManager()
self.clear_planning_scene_service = rospy.Service(
"/world_manager/clear_objects",
std_srvs.srv.Empty,
self.clear_objects_cb)
self.add_box = rospy.Service(
"/world_manager/add_box", world_manager_msgs.srv.AddBox,
self.add_box_cb)
self.add_mesh = rospy.Service(
"/world_manager/add_mesh", world_manager_msgs.srv.AddMesh,
self.add_mesh_cb)
self.add_tf_service = rospy.Service(
"/world_manager/add_tf", world_manager_msgs.srv.AddTF,
self.add_tf_cb)
self.add_walls_service = rospy.Service(
"/world_manager/add_walls", std_srvs.srv.Empty,
self.add_walls_cb)
rospy.sleep(1.0)
self.clear_objects_cb(request=None)
self.add_walls_cb(request=None)
rospy.loginfo("World Manager Node is Up and Running")
def add_mesh_cb(self, request):
so = request.scene_object
# add the tf
self.tf_manager.add_tf(so.object_name, so.pose_stamped)
# remove the old completion if it is there
self.scene.remove_world_object(so.object_name)
# add the new object to the planning scene
self.scene.add_mesh(so.object_name, so.pose_stamped,
so.mesh_filepath)
return []
def add_box_cb(self, request):
# type: (world_manager_msgs.srv.AddBoxRequest) -> []
box = request.scene_box
# type: box -> world_manager_msgs.msg.SceneBox
# add the tf
self.tf_manager.add_tf(box.object_name, box.pose_stamped)
# remove the old box if it is there
self.scene.remove_world_object(box.object_name)
# add the new box to the planning scene
self.scene.add_box(name=box.object_name,
pose=box.pose_stamped,
size=(box.edge_length_x, box.edge_length_y, box.edge_length_z))
rospy.loginfo("Added box {}".format(box.object_name))
return []
def add_tf_cb(self, request):
self.tf_manager.add_tf(request.frame_name, request.pose_stamped)
return []
def clear_objects_cb(self, request):
body_names = self.scene.get_known_object_names()
try:
walls = rospy.get_param('walls')
except:
rospy.loginfo("No wall in this scene")
rospy.loginfo("Clearing objects: {}".format(body_names))
for body_name in body_names:
if not body_name in walls:
rospy.loginfo("Removing object: {}".format(body_name))
self.scene.remove_world_object(body_name)
self.tf_manager.clear_tfs()
return []
def add_walls_cb(self, request):
try:
walls = rospy.get_param('walls')
except:
rospy.loginfo("No wall in this scene")
walls = []
for wall_params in walls:
rospy.loginfo("Adding wall " + str(wall_params))
self._add_wall(wall_params)
return []
def _add_wall(self, wall_params):
name = wall_params["name"]
x_thickness = wall_params["x_thickness"]
y_thickness = wall_params["y_thickness"]
z_thickness = wall_params["z_thickness"]
x = wall_params["x"]
y = wall_params["y"]
z = wall_params["z"]
qx = wall_params["qx"]
qy = wall_params["qy"]
qz = wall_params["qz"]
qw = wall_params["qw"]
frame_id = wall_params["frame_id"]
back_wall_pose = geometry_msgs.msg.PoseStamped()
back_wall_pose.header.frame_id = '/' + frame_id
wall_dimensions = [x_thickness, y_thickness, z_thickness]
back_wall_pose.pose.position = geometry_msgs.msg.Point(
**{'x': x,
'y': y,
'z': z})
back_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(
**{'x': qx,
'y': qy,
'z': qz,
'w': qw})
self.scene.add_box(name, back_wall_pose, wall_dimensions)
if __name__ == '__main__':
init_node()
group = MoveGroupCommander("right_arm")
# Initialize the Planning Scene
rospy.loginfo("Setting the Planning Scene...")
scene = PlanningSceneInterface()
rospy.sleep(2)
scene.remove_world_object() # Remove all objects first
rospy.sleep(2)
rospy.loginfo("All objects Removed : {}".format(
scene.get_known_object_names()))
# Pose Target 1
rospy.loginfo("Start Pose Target 1")
pose_target_1 = Pose()
pose_target_1.position.x = 0.3
pose_target_1.position.y = -0.1
pose_target_1.position.z = 0.15
pose_target_1.orientation.x = 0.0
pose_target_1.orientation.y = -0.707
pose_target_1.orientation.z = 0.0
pose_target_1.orientation.w = 0.707
rospy.loginfo("Set Target to Pose:\n{}".format(pose_target_1))
def main():
rospy.init_node('Baxter_Env')
robot = RobotCommander()
scene = PlanningSceneInterface()
scene._scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=0)
rospy.sleep(2)
# centre table
p1 = PoseStamped()
p1.header.frame_id = robot.get_planning_frame()
p1.pose.position.x = 1. # position of the table in the world frame
p1.pose.position.y = 0.
p1.pose.position.z = 0.
# left table (from baxter's perspective)
p1_l = PoseStamped()
p1_l.header.frame_id = robot.get_planning_frame()
p1_l.pose.position.x = 0.5 # position of the table in the world frame
p1_l.pose.position.y = 1.
p1_l.pose.position.z = 0.
p1_l.pose.orientation.x = 0.707
p1_l.pose.orientation.y = 0.707
# right table (from baxter's perspective)
p1_r = PoseStamped()
p1_r.header.frame_id = robot.get_planning_frame()
p1_r.pose.position.x = 0.5 # position of the table in the world frame
p1_r.pose.position.y = -1.
p1_r.pose.position.z = 0.
p1_r.pose.orientation.x = 0.707
p1_r.pose.orientation.y = 0.707
# open back shelf
p2 = PoseStamped()
p2.header.frame_id = robot.get_planning_frame()
p2.pose.position.x = 1.2 # position of the table in the world frame
p2.pose.position.y = 0.0
p2.pose.position.z = 0.75
p2.pose.orientation.x = 0.5
p2.pose.orientation.y = -0.5
p2.pose.orientation.z = -0.5
p2.pose.orientation.w = 0.5
pw = PoseStamped()
pw.header.frame_id = robot.get_planning_frame()
# add an object to be grasped
p_ob1 = PoseStamped()
p_ob1.header.frame_id = robot.get_planning_frame()
p_ob1.pose.position.x = .92
p_ob1.pose.position.y = 0.3
p_ob1.pose.position.z = 0.89
# the ole duck
p_ob2 = PoseStamped()
p_ob2.header.frame_id = robot.get_planning_frame()
p_ob2.pose.position.x = 0.87
p_ob2.pose.position.y = -0.4
p_ob2.pose.position.z = 0.24
# clean environment
scene.remove_world_object("table_center")
scene.remove_world_object("table_side_left")
scene.remove_world_object("table_side_right")
scene.remove_world_object("shelf")
scene.remove_world_object("wall")
scene.remove_world_object("part")
scene.remove_world_object("duck")
rospy.sleep(1)
scene.add_box("table_center", p1,
size=(.5, 1.5, 0.4)) # dimensions of the table
scene.add_box("table_side_left", p1_l, size=(.5, 1.5, 0.4))
scene.add_box("table_side_right", p1_r, size=(.5, 1.5, 0.4))
scene.add_mesh(
"shelf",
p2,
"/home/nikhildas/ros_ws/baxter_interfaces/baxter_moveit_config/baxter_scenes/bookshelf_light.stl",
size=(.025, .01, .01))
scene.add_plane("wall", pw, normal=(0, 1, 0))
part_size = (0.07, 0.05, 0.12)
scene.add_box("part", p_ob1, size=part_size)
scene.add_mesh(
"duck",
p_ob2,
"/home/nikhildas/ros_ws/baxter_interfaces/baxter_moveit_config/baxter_scenes/duck.stl",
size=(.001, .001, .001))
rospy.sleep(1)
print(scene.get_known_object_names())
## ---> SET COLOURS
table_color = color_norm([105, 105,
105]) # normalize colors to range [0, 1]
shelf_color = color_norm([139, 69, 19])
duck_color = color_norm([255, 255, 0])
_colors = {}
setColor('table_center', _colors, table_color, 1)
setColor('table_side_left', _colors, table_color, 1)
setColor('table_side_right', _colors, table_color, 1)
setColor('shelf', _colors, shelf_color, 1)
setColor('duck', _colors, duck_color, 1)
sendColors(_colors, scene)
def main():
# moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('get_position_client')
scene = PlanningSceneInterface()
objects = scene.get_known_object_names()
# print("objects", objects)
# print("scene", scene)
left_arm = MoveGroupCommander("left_arm")
# left_gripper_group = moveit_commander.MoveGroupCommander("left_gripper")
right_arm = MoveGroupCommander("right_arm")
# right_gripper = moveit_commander.MoveGroupCommander("right_gripper")
dual_arm_group = MoveGroupCommander("dual_arm")
grippers = MoveGroupCommander("grippers")
get_position = rospy.ServiceProxy('get_position', target_position)
rate = rospy.Rate(2.0)
done_l = False
done_r = False
# dual_joints = dual_arm_group.get_joints()
# print("dual_joints" , dual_joints)
# dual_joint_values = dual_arm_group.get_current_joint_values()
# print("dual_joint_values", dual_joint_values)
# left_arm_group.set_named_target("left_home")
# plan1 = left_arm_group.plan()
# rospy.sleep(2)
# left_arm_group.go()
# print "...Left Done..."
# rospy.sleep(1)
dual_arm_group.set_named_target("both_home")
plan1 = dual_arm_group.plan()
rospy.sleep(2)
# print("dual_arm plan1", plan1)
dual_arm_group.go()
print "...Both Done..."
rospy.sleep(1)
# print("scene", scene)
while not rospy.is_shutdown():
try:
rospy.wait_for_service('get_position', timeout=5)
res = get_position()
left_handle = res.points[0]
right_handle = res.points[1]
# scene.remove_world_object()
# print "right_handle" , right_handle
# pose_target_l = []
# pose_target_l.append(( left_handle.x - 0.03))
# pose_target_l.append(left_handle.y)
# pose_target_l.append(left_handle.z)
# pose_target_r = []
# pose_target_r.append(right_handle.x)
# pose_target_r.append(right_handle.y)
# pose_target_r.append(right_handle.z)
# print(pose_target_l)
print('left_handle', left_handle, 'right_handle', right_handle)
# left_arm_group.set_position_target(pose_target_l)
pose_target_l = Pose()
pose_target_r = Pose()
# pose_target_l.position = left_handle;
# pose_target_l.position.x -= 0.06
# pose_target_l.position.y -= 0.005
# pose_target_l.position.z -= 0.005
# q = quaternion_from_euler(-1.57, 0, -1.57) # Horizental Gripper : paraller to x-axis
# # q = quaternion_from_euler(-2.432, -1.57, -0.709) # Vertical Gripper: Vertical to x-axis
# pose_target_l.orientation.x = q[0]
# pose_target_l.orientation.y = q[1]
# pose_target_l.orientation.z = q[2]
# pose_target_l.orientation.w = q[3]
# print ("pose_target_l",pose_target_l )
# left_arm_group.set_pose_target(pose_target_l)
# left_arm_group.set_planning_time(10)
# plan1 = left_arm_group.plan()
# rospy.sleep(2)
# left_arm_group.go()
# done_l = True
# pose_target_r.position.x = 0.5232
# pose_target_r.position.y = -0.2743
# pose_target_r.position.z = 0.6846
# right_gripper.set_named_target("right_open")
# right_gripper.plan()
# right_gripper.go()
pose_target_r.position = right_handle
translation = get_translations(right_handle)
# pose_target_r.position.x -= 0.065
pose_target_r.position.x -= translation.x
print("translation.x", translation.x)
pose_target_r.position.y -= translation.y
pose_target_r.position.z -= translation.z
# pose_target_r.position.z = 0.7235
# q = quaternion_from_euler(-1.57, 0, -1.57) # This works from 2.35 : Gripper paraller to x-axis
q = quaternion_from_euler(
-1.57, 1.57, -1.57
) # This work from 2.35 : Gripper Vertical to x-axis (-2.36, 1.5708, -2.36)
pose_target_r.orientation.x = q[0]
pose_target_r.orientation.y = q[1]
pose_target_r.orientation.z = q[2]
pose_target_r.orientation.w = q[3]
# print ("pose_target_r",pose_target_r )
right_arm.set_pose_target(pose_target_r)
right_arm.set_planning_time(10)
plan1 = right_arm.plan()
# rospy.sleep(1)
# right_arm.go()
if done_r == False:
# print ("target pose",pose_target_r)
if (plan1.joint_trajectory.points
): # True if trajectory contains points
# last = (len(plan1.joint_trajectory.points) - 1)
# print("joint_trajectory", (plan1.joint_trajectory.points[-1].positions) ) # getting the last position of trajectory
r_joints = plan1.joint_trajectory.points[-1].positions
d_joints = get_dual_trajectory(r_joints)
# print("l_joints", l_joints)
# d_joints = l_joints + list(r_joints)
# print ("d_joints", d_joints)
dual_arm_group.set_joint_value_target(d_joints)
plan2 = dual_arm_group.plan()
if (plan2.joint_trajectory.points):
move_success = dual_arm_group.execute(plan2, wait=True)
# eef_pose = right_arm.get_current_pose()
# print("eef_pose", eef_pose)
# move_success = right_arm.execute(plan1, wait = True)
if move_success == True:
rospy.sleep(2)
right_arm.set_start_state_to_current_state()
left_arm.set_start_state_to_current_state()
waypoints_l = []
waypoints = []
wpose = right_arm.get_current_pose().pose
wpose_l = left_arm.get_current_pose().pose
print("wpose", wpose)
# Open the gripper to the full position
grippers.set_named_target("both_open")
grippers.plan()
grippers.go()
# Create Cartesian Path to move forward mainting the end-effector pose
# waypoints = []
# rospy.sleep(5)
# wpose = right_arm.get_current_pose().pose
if (translation.x >= 0.0505):
wpose.position.x += (translation.x + 0.00
) # move forward in (x)
wpose_l.position.x += (translation.x + 0.002)
wpose_l.position.z += 0.001
else:
wpose.position.x += 0.051 # move forward in (x)
wpose_l.position.x += 0.053
wpose_l.position.z += 0.001
waypoints.append(deepcopy(wpose))
(plan1,
fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0)
waypoints_l.append(deepcopy(wpose_l))
(plan_l,
fraction) = left_arm.compute_cartesian_path(
waypoints_l, # waypoints to follow
0.01, # eef_step
0.0)
# print("plan1 len", len(plan1.joint_trajectory.points))
# waypoints.append(copy.deepcopy(wpose))
# (plan2, fraction) = dual_arm_group.compute_cartesian_path(
# waypoints, # waypoints to follow
# 0.005, # eef_step
# 0.0)
rospy.sleep(1)
if plan1.joint_trajectory.points:
move_success = right_arm.execute(plan1,
wait=True)
if move_success == True:
rospy.loginfo(
"Right Move forward successful")
# done_r = True
# break;
if plan_l.joint_trajectory.points:
move_success_l = left_arm.execute(plan_l,
wait=True)
if move_success_l == True:
rospy.loginfo(
"Left Move forward successful")
# done_r = True
if move_success == True and move_success_l == True:
rospy.sleep(1)
grippers.set_named_target("both_close")
grippers.plan()
grippers.go()
# rospy.sleep(1)
waypoints = []
rospy.sleep(1)
wpose = right_arm.get_current_pose().pose
# q = quaternion_from_euler(-1.57, 1.57, -1.57) # wrist up 5 degrees = 1.66 10deg = 1.75
# wpose.orientation.x = q[0]
# wpose.orientation.y = q[1]
# wpose.orientation.z = q[2]
# wpose.orientation.w = q[3]
wpose.position.z += 0.07 # move up in (z)
waypoints.append(deepcopy(wpose))
(plan1,
fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0)
if plan1.joint_trajectory.points:
# move_success = right_arm.execute(plan1, wait=True) # for testing right arm trajectory
# done_r = True
r_joints = plan1.joint_trajectory.points[
-1].positions
# print ("r_joints", r_joints)
d_joints = get_dual_trajectory(r_joints)
# print ("d_joints", d_joints)
dual_arm_group.set_joint_value_target(
d_joints)
# plan2 = 0
plan2 = dual_arm_group.plan()
# print("planed plan len", len(plan2.joint_trajectory.points))
# print("dual arm trajectory", (plan2.joint_trajectory.points))
# done_r = True
if (plan2.joint_trajectory.points):
move_success = dual_arm_group.go()
print("move_success", move_success)
done_r = True
# traj_points = plan2.joint_trajectory.points[0:-3]
# plan2.joint_trajectory.points = traj_points
# print("eddited plan len", len(plan2.joint_trajectory.points))
# move_success = dual_arm_group.execute(plan2, wait = True)
# # done_r = True
# move_success = right_arm.execute(plan1, wait=True)
# if move_success == True:
# rospy.loginfo ("Lift Successful")
# done_r = True
# r_joints = plan1.joint_trajectory.points[-1].positions
# d_joints = get_dual_trajectory(r_joints)
# # print("l_joints", l_joints)
# # d_joints = l_joints + list(r_joints)
# # print ("d_joints", d_joints)
# dual_arm_group.set_joint_value_target(d_joints)
# plan2 = dual_arm_group.plan()
# if (plan2.joint_trajectory.points) :
# move_success = dual_arm_group.execute(plan2, wait = True)
# if move_success == True:
# rospy.loginfo("Move forward successful")
# rospy.sleep(1)
# grippers.set_named_target("both_close")
# grippers.plan()
# grippers.go()
# rospy.sleep(1)
# waypoints = []
# rospy.sleep(2)
# wpose = right_arm.get_current_pose().pose
# wpose.position.z += 0.1 # move up in (z)
# waypoints.append(copy.deepcopy(wpose))
# (plan1, fraction) = right_arm.compute_cartesian_path(
# waypoints, # waypoints to follow
# 0.01, # eef_step
# 0.0)
# if plan1.joint_trajectory.points:
# r_joints = plan1.joint_trajectory.points[-1].positions
# d_joints = get_dual_trajectory(r_joints)
# # print ("d_joints", d_joints)
# dual_arm_group.set_joint_value_target(d_joints)
# plan2 = dual_arm_group.plan()
# if (plan2.joint_trajectory.points) :
# move_success = dual_arm_group.execute(plan2, wait = True)
# done_r = True
# move_success = right_arm.execute(plan1, wait=True)
# if move_success == True:
# rospy.loginfo ("Lift Successful")
# done_r = True
# else:
# rospy.logwarn("Cartesian Paht Planning Failed for forward movement")
else:
print("Execution Completed")
except rospy.ROSException:
rospy.logerr('get_position_server did not respond in 5 sec')
return
rate.sleep()
class CollisionEnv(object):
NS = "/art/collision_env/"
def __init__(self, setup, world_frame):
assert setup != ""
self.ready = False
self.setup = setup
self.world_frame = world_frame
self.tf_listener = TransformListener()
self.api = ArtApiHelper()
rospy.loginfo("Waiting for DB API")
self.api.wait_for_db_api()
self.ignored_prefixes = array_from_param("~ignored_prefixes")
rospy.loginfo("Will ignore following prefixes: " +
str(self.ignored_prefixes))
self.lock = RLock()
self.ps = PlanningSceneInterface()
self._paused = False
self.oh = ObjectHelper(self.object_cb)
self.artificial_objects = {}
self.collision_objects_pub = rospy.Publisher(self.NS + "artificial",
CollisionObjects,
latch=True,
queue_size=1)
self.pub_artificial()
self.timer = rospy.Timer(rospy.Duration(1.0), self.timer_cb)
self.paused_pub = rospy.Publisher(self.NS + "paused",
Bool,
latch=True,
queue_size=1)
self.paused = False
def start(self):
self.ready = True
rospy.loginfo("Ready")
def load_from_db(self):
for prim in self.api.get_collision_primitives(self.setup):
rospy.loginfo("Loading object: " + prim.name)
self.add_primitive(prim)
def save_primitive(self, name):
with self.lock:
if name not in self.artificial_objects:
raise CollisionEnvException("Unknown object name")
p = self.artificial_objects[name]
self.api.add_collision_primitive(p)
def save_primitives(self):
with self.lock:
for name in self.artificial_objects.keys():
self.save_primitive(name)
def set_primitive_pose(self, name, ps):
with self.lock:
p = self.artificial_objects[name]
assert p.pose.header.frame_id == ps.header.frame_id
p.pose.pose = ps.pose
self.ps.add_box(p.name, p.pose, p.bbox.dimensions)
self.pub_artificial()
def add_primitive(self, p):
with self.lock:
self.artificial_objects[p.name] = p
self.ps.add_box(p.name, p.pose, p.bbox.dimensions)
self.pub_artificial()
def pub_artificial(self):
msg = CollisionObjects()
for v in self.artificial_objects.values():
# transform all collision primitives into world frame (= marker)
if v.pose.header.frame_id != self.world_frame:
try:
self.tf_listener.waitForTransform(v.pose.header.frame_id,
self.world_frame,
rospy.Time(0),
rospy.Duration(1.0))
v.pose = self.tf_listener.transformPose(
self.world_frame, v.pose)
except tf.Exception as e:
rospy.logwarn("Failed to transform artificial object: " +
str(e))
continue
msg.primitives.append(v)
self.collision_objects_pub.publish(msg)
@property
def paused(self):
return self._paused
@paused.setter
def paused(self, val):
self._paused = val
self.paused_pub.publish(val)
def remove_name(self, name):
with self.lock:
if name not in self.artificial_objects:
rospy.logwarn("Unknown artificial object: " + name)
return False
self.ps.remove_world_object(name)
del self.artificial_objects[name]
self.pub_artificial()
if not self.api.clear_collision_primitives(self.setup, names=[name]):
rospy.logwarn("Failed to remove from permanent storage")
rospy.loginfo("Removed object: " + name)
return True
def clear_all(self, permanent=True):
with self.lock:
rospy.loginfo("Clearing " + str(len(self.artificial_objects)) +
" artificial objects...")
for k in self.artificial_objects.keys():
if self.is_ignored(k):
continue
self.ps.remove_world_object(k)
self.artificial_objects = {}
self.pub_artificial()
try:
if permanent and not self.api.clear_collision_primitives(
self.setup):
rospy.logwarn("Failed to remove from permanent storage")
except ArtApiException as e:
rospy.logerr(str(e))
def reload(self):
self.clear_all(permanent=False)
self.load_from_db()
self.pub_artificial()
def _generate_name(self):
# as we use only part of uuid, there might be collisions...
while True:
name = str(uuid.uuid4())[:8]
if name not in self.artificial_objects.keys():
break
return name
def set_det_pose(self, name, ps):
object_type = self.api.get_object_type(
self.oh.objects[name].object_type)
if object_type is not None:
self.add_detected(name, ps, object_type)
def clear_det_on_table(self, inv=False, ignore=None):
if ignore is None:
ignore = []
ret = []
with self.lock:
for v in self.oh.objects.values():
if v.object_id in ignore:
continue
if not inv and not v.on_table:
continue
if inv and v.on_table:
continue
ret.append(v.object_id)
self.clear_detected(v.object_id)
return ret
def is_ignored(self, name):
for ip in self.ignored_prefixes:
if name.startswith(ip):
return True
return False
def timer_cb(self, evt):
if self.paused:
return
with self.lock:
known_objects = self.ps.get_known_object_names()
for name in known_objects:
if name not in self.artificial_objects and name not in self.oh.objects and not self.is_ignored(
name):
rospy.loginfo("Removing outdated object: " + name)
self.clear_detected(name)
# restore artificial objects if they are lost somehow (e.g. by restart of MoveIt!)
for k, v in self.artificial_objects.iteritems():
if k in known_objects:
continue
rospy.loginfo("Restoring artificial object: " + v.name)
if v.bbox.type == SolidPrimitive.BOX:
self.ps.add_box(v.name, v.pose, v.bbox.dimensions)
else:
# TODO other types
pass
def object_cb(self, evt, h, inst):
if self.paused or not self.ready:
return
with self.lock:
attached_objects = self.ps.get_attached_objects()
if evt in (ObjectHelper.OBJECT_ADDED, ObjectHelper.OBJECT_UPDATED):
if inst.object_id in attached_objects:
return
ps = PoseStamped()
ps.header = h
ps.pose = inst.pose
object_type = self.api.get_object_type(inst.object_type)
if object_type is not None:
self.add_detected(inst.object_id, ps, object_type)
elif evt == ObjectHelper.OBJECT_LOST:
if inst.object_id not in attached_objects:
self.clear_detected(inst.object_id)
def add_detected(self, name, ps, object_type):
with self.lock:
self.ps.add_box(name, ps, object_type.bbox.dimensions)
def clear_detected(self, name):
with self.lock:
self.ps.remove_world_object(name)
def clear_all_det(self, ignore=None):
if ignore is None:
ignore = []
ret = []
with self.lock:
for v in self.oh.objects.values():
name = v.object_id
if name in ignore:
continue
self.clear_detected(name)
ret.append(name)
rospy.loginfo("Removed " + str(len(ret)) + " detected objects.")
return ret
def get_attached(self, transform_to_world=True):
"""
keep in mind - attached objects might not be detected
"""
ret = []
with self.lock:
ao = self.ps.get_attached_objects()
for k, v in ao.iteritems():
if len(v.object.primitives) != 1 or len(
v.object.primitive_poses) != 1:
rospy.logwarn("Unsupported type of attached object: " + k)
continue
if v.object.primitives[0].type != SolidPrimitive.BOX:
rospy.logwarn(
"Only box-like attached objects are supported so far.")
continue
ps = PoseStamped()
ps.header = v.object.header
ps.pose = v.object.primitive_poses[0]
if transform_to_world and ps.header.frame_id != self.world_frame:
try:
self.tf_listener.waitForTransform(ps.header.frame_id,
self.world_frame,
ps.header.stamp,
rospy.Duration(1.0))
ps = self.tf_listener.transformPose(self.world_frame, ps)
except tf.Exception as e:
rospy.logwarn("Failed to transform attached object: " +
str(e))
continue
ret.append((k, ps, v.object.primitives[0]))
return ret
class PickAndPlace:
def __init__(self, robot_name="panda_arm", frame="panda_link0"):
try:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node(name="pick_place_test")
self.scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# region Robot initial
self.robot = MoveGroupCommander(robot_name)
self.robot.set_goal_joint_tolerance(0.00001)
self.robot.set_goal_position_tolerance(0.00001)
self.robot.set_goal_orientation_tolerance(0.01)
self.robot.set_goal_tolerance(0.00001)
self.robot.allow_replanning(True)
self.robot.set_pose_reference_frame(frame)
self.robot.set_planning_time(3)
# endregion
self.gripper = MoveGroupCommander("hand")
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_OPEN)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
# Robot go home
self.robot.set_named_target("home")
self.robot.go()
# clear all object in world
self.clear_all_object()
table_pose = un.Pose(0, 0, -10, 0, 0, 0)
table_color = un.Color(255, 255, 0, 100)
self.add_object_box("table", table_pose, table_color, frame,
(2000, 2000, 10))
bearing_pose = un.Pose(250, 250, 500, -90, 45, -90)
bearing_color = un.Color(255, 0, 255, 255)
bearing_file_name = "../stl/bearing.stl"
self.add_object_mesh("bearing", bearing_pose, bearing_color, frame,
bearing_file_name)
obpose = self.scene.get_object_poses(["bearing"])
# self.robot.set_support_surface_name("table")
g = Grasp()
# Create gripper position open or close
g.pre_grasp_posture = self.open_gripper()
g.grasp_posture = self.close_gripper()
g.pre_grasp_approach = self.make_gripper_translation(
0.01, 0.1, [0, 1.0, 0])
g.post_grasp_retreat = self.make_gripper_translation(
0.01, 0.9, [0, 1.0, 0])
p = PoseStamped()
p.header.frame_id = "panda_link0"
p.pose.orientation = obpose["bearing"].orientation
p.pose.position = obpose["bearing"].position
g.grasp_pose = p
g.allowed_touch_objects = ["bearing"]
a = []
a.append(g)
result = self.robot.pick(object_name="bearing", grasp=a)
print(result)
except Exception as ex:
print(ex)
moveit_commander.roscpp_shutdown()
moveit_commander.roscpp_shutdown()
def make_gripper_translation(self, min_dist, desired, vector):
g = GripperTranslation()
g.direction.vector.x = vector[0]
g.direction.vector.y = vector[1]
g.direction.vector.z = vector[2]
g.direction.header.frame_id = "panda_link0"
g.min_distance = min_dist
g.desired_distance = desired
return g
def open_gripper(self):
t = JointTrajectory()
t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
tp = JointTrajectoryPoint()
tp.positions = [0.04, 0.04]
tp.time_from_start = rospy.Duration(secs=1)
t.points.append(tp)
return t
def close_gripper(self):
t = JointTrajectory()
t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
tp = JointTrajectoryPoint()
tp.positions = [0.0, 0.0]
tp.time_from_start = rospy.Duration(secs=1)
t.points.append(tp)
return t
def clear_all_object(self):
for world_object in (self.scene.get_known_object_names()):
self.scene.remove_world_object(world_object)
for attached_object in (self.scene.get_attached_objects()):
self.scene.remove_attached_object(attached_object)
def add_object_box(self, object_name, pose, color, frame, size):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param size: size of object(mm)
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_box(name=object_name,
pose=object_pose,
size=(size[0] / 1000.0, size[1] / 1000.0,
size[2] / 1000.0))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
def add_object_mesh(self, object_name, pose, color, frame, file_name):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param file_name: mesh file name
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_mesh(name=object_name,
pose=object_pose,
filename=file_name,
size=(0.001, 0.001, 0.001))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
class Scene:
interface = None # type: PlanningSceneInterface
publisher = None # type: rospy.Publisher
ref_frame = '' # type: str
colors = dict() # type: dict
default_color = (0.75, 0.75, 0.75, 1) # type: tuple
def __init__(self, ref_frame='base_link'):
self.interface = PlanningSceneInterface()
self.publisher = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
self.ref_frame = ref_frame
rospy.sleep(1)
def destroy(self):
pass
# ---- add & remove ---- #
def add_box(self, name, size, transform, rgba=None):
if rgba is None:
rgba = self.default_color
self.interface.remove_world_object(name)
pose = PoseStamped()
pose.header.frame_id = self.ref_frame
pose.pose = frame_to_pose(transform)
self.interface.add_box(name, pose, size)
self.wait_state_update(name)
self.set_color(name, rgba)
self.send_colors()
def remove_object(self, name):
self.interface.remove_world_object(name)
self.wait_state_update(name, is_known=False)
# ---- utils ---- #
def set_color(self, name, rgba):
color = ObjectColor()
color.id = name
color.color.r = rgba[0]
color.color.g = rgba[1]
color.color.b = rgba[2]
color.color.a = rgba[3]
self.colors[name] = color
def send_colors(self):
p = PlanningScene()
p.is_diff = True
for color in self.colors.values():
p.object_colors.append(color)
self.publisher.publish(p)
def wait_state_update(self, name, is_known=True, is_attached=False, timeout=4):
start = rospy.get_time()
current = rospy.get_time()
while (current - start < timeout) and not rospy.is_shutdown():
attached_objects = self.interface.get_attached_objects([name])
cur_is_attached = len(attached_objects.keys()) > 0
cur_is_known = name in self.interface.get_known_object_names()
if (cur_is_attached == is_attached) and (cur_is_known == is_known):
rospy.loginfo('Scene update succeeded.')
return True
rospy.sleep(0.1)
current = rospy.get_time()
# rospy.loginfo("Wait scene update [" + str(current - start) + "s]...")
rospy.loginfo('Scene update failed')
return False
class WorldManager:
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.model_pose_broadcaster = ModelPoseBroadcaster()
# self.clear_planning_scene_service = rospy.Service("/world_manager/clear_planning_scene", graspit_msgs.srv.ClearModels, self.remove_all_objects_from_planner)
self.add_mesh_to_planning_scene_service = rospy.Service(
"/world_manager/add_object", graspit_msgs.srv.AddObject,
self.add_object_to_planning_scene)
rospy.sleep(1.0)
# self.add_walls()
rospy.loginfo("World Manager Node is Up and Running")
def add_object_to_planning_scene(self, request):
self.remove_all_objects_from_planner(None)
#this makes sure tf is continually broadcast
self.model_pose_broadcaster.add_model(request.objectname,
request.pose_stamped)
#remove the old completion if it is there
self.scene.remove_world_object(request.objectname)
#add the new object to the planning scene
self.scene.add_mesh(request.objectname, request.pose_stamped,
request.mesh_filepath)
return []
def remove_all_objects_from_planner(self, request):
body_names = self.scene.get_known_object_names()
while len(body_names) > 0:
print(
"removing bodies from the planner, this can potentially take several tries"
)
for body_name in body_names:
self.scene.remove_world_object(body_name)
body_names = self.scene.get_known_object_names()
self.model_pose_broadcaster.clear_models()
# self.add_walls()
return []
def add_walls(self):
walls = rospy.get_param('/walls')
for wall_params in walls:
rospy.loginfo("Adding wall " + str(wall_params))
self.add_wall(wall_params)
def add_wall(self, wall_params):
name = wall_params["name"]
x_thickness = wall_params["x_thickness"]
y_thickness = wall_params["y_thickness"]
z_thickness = wall_params["z_thickness"]
x = wall_params["x"]
y = wall_params["y"]
z = wall_params["z"]
qx = wall_params["qx"]
qy = wall_params["qy"]
qz = wall_params["qz"]
qw = wall_params["qw"]
frame_id = wall_params["frame_id"]
back_wall_pose = geometry_msgs.msg.PoseStamped()
back_wall_pose.header.frame_id = '/' + frame_id
wall_dimensions = [x_thickness, y_thickness, z_thickness]
back_wall_pose.pose.position = geometry_msgs.msg.Point(**{
'x': x,
'y': y,
'z': z
})
back_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{
'x': qx,
'y': qy,
'z': qz,
'w': qw
})
self.scene.add_box(name, back_wall_pose, wall_dimensions)
