def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = ExtendedPlanningSceneInterface()
"""
:type self.robot :moveit_commander.RobotCommander
"""
self.robot = moveit_commander.RobotCommander()
#model_rec_manager for all the objects in the enviornment
self.model_manager = ModelRecManager()
#a cache of all of the object names in the enviorment, for use with remove_all_objects
self.body_name_cache = []
self.add_box_server = rospy.Service('moveit_trajectory_planner/add_box',
BoxInfo,
self.handle_add_box)
self.add_autoscaled_mesh_server = rospy.Service('moveit_trajectory_planner/add_autoscaled_mesh',
MeshInfo,
self.handle_add_autoscaled_mesh)
self.remove_object_server = rospy.Service('moveit_trajectory_planner/remove_object',
ObjectName,
self.handle_remove_object)
self.refresh_model_list_server = rospy.Service('model_manager/refresh_model_list',
Empty,
self.refresh_model_list)
self.reload_model_list_server = rospy.Service('model_manager/reload_model_list',
Empty,
self.return_current_model_list)
self.planning_scene_service = rospy.ServiceProxy("/get_planning_scene", moveit_msgs.srv.GetPlanningScene)
self.update_planning_scene_from_moveit()
self.force_use_moveit = rospy.get_param("use_moveit", 1)
self._run_recognition_as = actionlib.SimpleActionServer("recognize_objects_action",
graspit_msgs.msg.RunObjectRecognitionAction,
execute_cb=self._run_recognition_as_cb,
auto_start=False)
self._run_recognition_as.start()
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.NEW_MODEL_REC = True
self.scene = ExtendedPlanningSceneInterface()
self.robot = moveit_commander.RobotCommander()
self.model_manager = ModelRecManager(self.NEW_MODEL_REC)
self.planning_scene_service_proxy = rospy.ServiceProxy("/get_planning_scene", moveit_msgs.srv.GetPlanningScene)
self._run_recognition_as = actionlib.SimpleActionServer("recognize_objects_action",
graspit_msgs.msg.RunObjectRecognitionAction,
execute_cb=self._run_recognition_as_cb,
auto_start=False)
self._run_recognition_as.start()
rospy.loginfo("World Manager Node is Up and Running")
class WorldManager:
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = ExtendedPlanningSceneInterface()
"""
:type self.robot :moveit_commander.RobotCommander
"""
self.robot = moveit_commander.RobotCommander()
#model_rec_manager for all the objects in the enviornment
self.model_manager = ModelRecManager()
#a cache of all of the object names in the enviorment, for use with remove_all_objects
self.body_name_cache = []
self.add_box_server = rospy.Service('moveit_trajectory_planner/add_box',
BoxInfo,
self.handle_add_box)
self.add_autoscaled_mesh_server = rospy.Service('moveit_trajectory_planner/add_autoscaled_mesh',
MeshInfo,
self.handle_add_autoscaled_mesh)
self.remove_object_server = rospy.Service('moveit_trajectory_planner/remove_object',
ObjectName,
self.handle_remove_object)
self.refresh_model_list_server = rospy.Service('model_manager/refresh_model_list',
Empty,
self.refresh_model_list)
self.reload_model_list_server = rospy.Service('model_manager/reload_model_list',
Empty,
self.return_current_model_list)
self.planning_scene_service = rospy.ServiceProxy("/get_planning_scene", moveit_msgs.srv.GetPlanningScene)
self.update_planning_scene_from_moveit()
self.force_use_moveit = rospy.get_param("use_moveit", 1)
self._run_recognition_as = actionlib.SimpleActionServer("recognize_objects_action",
graspit_msgs.msg.RunObjectRecognitionAction,
execute_cb=self._run_recognition_as_cb,
auto_start=False)
self._run_recognition_as.start()
def update_planning_scene_from_moveit(self):
""" Update the current cached scene directly from moveit.
Returns true if it the service call succeeds. False implies moveit hasn't started correctly.
"""
try:
rospy.wait_for_service("/get_planning_scene", 5)
components = PlanningSceneComponents(PlanningSceneComponents.WORLD_OBJECT_NAMES + PlanningSceneComponents.TRANSFORMS)
"""
:type ps_response :moveit_msgs.srv.GetPlanningSceneResponse
"""
ps_request = moveit_msgs.srv.GetPlanningSceneRequest(components=components)
ps_response = self.planning_scene_service.call(ps_request)
self.body_name_cache = [co.id for co in ps_response.scene.world.collision_objects]
rospy.loginfo("body name cache:%s"%(', '.join(self.body_name_cache)))
return True
except Exception as e:
raise e
rospy.logerror("Failed to find service /get_planning_scene %s and force use moveit enabled"%(e))
rospy.shutdown()
rospy.logwarn("Failed to find service /get_planning_scene %s"%(e))
return False
def handle_add_box(self, req):
box_dimensions = (req.sizeX, req.sizeY, req.sizeZ)
self.scene.add_box(req.name, req.pose, box_dimensions)
return BoxInfoResponse()
def handle_add_autoscaled_mesh(self, req):
"""
Adds a mesh, but makes sure that the mesh is scaled to meters if given a mesh that
is in millimeters.
"""
if os.path.isfile(req.filename):
self.scene.add_mesh_autoscaled(req.name, req.pose, req.filename)
rospy.loginfo(self.__class__.__name__ + '::handle_add_autoscaled_mesh::name -- %s filename -- %s'%(req.name, req.filename) )
else:
rospy.logwarn('File doesn\'t exist - object %s, filename %s'%(req.name, req.filename))
return MeshInfoResponse()
def handle_remove_object(self, req):
self.scene.remove_world_object(req.name)
return ObjectNameResponse()
def refresh_model_list(self, empty_msg):
self.model_manager.refresh()
#self.remove_object_publisher.publish('ALL') #commented lines for graspit
#self.publish_table_models()
self.remove_all_objects_from_planner()
self.add_all_objects_to_planner()
#need to return [] for empty response
return []
def _run_recognition_as_cb(self, goal):
rospy.loginfo("Received Run Recognition Goal")
self.remove_all_objects_from_planner()
self.model_manager.refresh()
#self.remove_object_publisher.publish('ALL') #commented lines for graspit
#self.publish_table_models()
self.add_all_objects_to_planner()
#need to return [] for empty response.
_result = trajectory_planner_msgs.msg.RunObjectRecognitionResult()
for model in self.model_manager.model_list:
model_name = model.model_name
object_name = model.object_name
object_pose = model.get_world_pose()
object_info = graspit_msgs.msg.ObjectInfo(object_name, model_name, object_pose)
rospy.loginfo("Object Info: " + str(object_info))
_result.object_info.append(object_info)
self._run_recognition_as.set_succeeded(_result)
return []
def return_current_model_list(self, empty_msg):
#self.model_manager.read()
#self.remove_object_publisher.publish('ALL') #commented lines for graspit
#self.publish_table_models()
if not len(self.model_manager.model_list):
self.refresh_model_list(empty_msg)
return []
self.remove_all_objects_from_planner()
self.add_all_objects_to_planner()
return []
def get_model_list(self, empty_msg):
self.world_manager.read()
def remove_all_objects_from_planner(self):
"""
@brief - Clears all models from moveit enivornment (from name cache)
FIXME - Add additional obstacles for camera post and computers around robot
and some way of annotating that they shouldn't be removed.
"""
# update the body name cache
self.update_planning_scene_from_moveit()
for index in range(len(self.body_name_cache)):
body_name = self.body_name_cache[index]
rospy.logwarn("removing body: body_name %s"%(body_name))
self.scene.remove_world_object(body_name)
#del self.body_name_cache[index]
#del body_name
self.body_name_cache = []
# read the obstacles such as the table and any safety walls
self.add_obstacles()
def add_all_objects_to_planner(self):
"""
@brief - Adds all of the models in the model_rec_manager to moveit enviornment and adds names to cache
"""
# Clear moveit's model list
for model in self.model_manager.model_list:
model_name = model.model_name.strip('/')
filename = file_name_dict[model_name]
rospy.logwarn('model name: {}, filename: {}'.format(model_name, filename))
if os.path.isfile(filename):
stampedModelPose = geometry_msgs.msg.PoseStamped()
stampedModelPose.header.frame_id = "/world" #"/camera_link" #self.robot.get_planning_frame()
rospy.loginfo(self.__class__.__name__ +
':: Adding model %s -- frame_id %s -- '%(model_name, stampedModelPose.header.frame_id) +
' filename %s '%(filename))
stampedModelPose.pose = model.get_world_pose()
self.scene.add_mesh_autoscaled(model.object_name, stampedModelPose, filename)
#self.scene.remove_world_object(model.object_name)
else:
rospy.logwarn('File doesn\'t exist - object %s, filename %s'%(model.object_name, filename))
def add_table(self):
time.sleep(1)
rospy.wait_for_service('moveit_trajectory_planner/add_box')
frame_id = "/world"
rospy.loginfo("adding table in planning frame: " + str(frame_id))
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = frame_id
table_x = rospy.get_param('/table_x', 1.45)
table_y = rospy.get_param('/table_y', .74)
table_z = rospy.get_param('/table_z', .05)
table_world_x_offset = rospy.get_param('/table_world_x_offset', -.05)
table_world_y_offset = rospy.get_param('/table_world_y_offset', -.05)
table_world_z_offset = rospy.get_param('/table_world_z_offset', -.0525)
box_pose.pose.position.x = table_world_x_offset
box_pose.pose.position.y = table_world_y_offset
box_pose.pose.position.z = table_world_z_offset
box_pose.pose.orientation.x = 0
box_pose.pose.orientation.y = 0
box_pose.pose.orientation.z = 0
box_pose.pose.orientation.w = 0
box_dimensions = (table_x, table_y, table_z)
self.scene.attach_box(world_manager.robot.get_link_names()[0], "table", box_pose, box_dimensions)
rospy.loginfo("table added")
def add_mico_base(self):
time.sleep(1)
rospy.wait_for_service('moveit_trajectory_planner/add_box')
frame_id = "/root"
rospy.loginfo("adding table in planning frame: " + str(frame_id))
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = frame_id
base_x = rospy.get_param('/base_x', 0.2)
base_y = rospy.get_param('/base_y', 0.15)
base_z = rospy.get_param('/base_z', 0.01)
base_robot_x_offset = rospy.get_param('/base_robot_x_offset', 0)
base_robot_y_offset = rospy.get_param('/base_robot_y_offset', 0)
base_robot_z_offset = rospy.get_param('/base_robot_z_offset', -0.02)
box_pose.pose.position.x = base_robot_x_offset
box_pose.pose.position.y = base_robot_y_offset
box_pose.pose.position.z = base_robot_z_offset
box_pose.pose.orientation.x = 0
box_pose.pose.orientation.y = 0
box_pose.pose.orientation.z = 0
box_pose.pose.orientation.w = 0
box_dimensions = (base_x, base_y, base_z)
self.scene.attach_box(world_manager.robot.get_link_names()[1], "robot_base", box_pose, box_dimensions)
rospy.loginfo("base added")
def add_walls(self):
back_wall_pose = geometry_msgs.msg.PoseStamped()
back_wall_pose.header.frame_id = '/staubli_rx60l_link1'
wall_dimensions = [0.92, 1.22, 0.05]
back_wall_pose.pose.position = geometry_msgs.msg.Point(**{'x': 0.35, 'y': -0.09, 'z': -0.22})
back_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': -0.7128395185,
'y': 0.0414512120285,
'z': 0.699774446448,
'w': 0.0213855555098})
left_wall_pose = geometry_msgs.msg.PoseStamped()
left_wall_pose.header.frame_id = '/staubli_rx60l_link1'
left_wall_dimensions = [0.92, 1.22, 0.05]
left_wall_pose.pose.position = geometry_msgs.msg.Point(**{'x': -0.56,
'y': -0.91,
'z': -0.09})
left_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': -0.482199130451,
'y': 0.516804171535,
'z': 0.487346836672,
'w': 0.512728493124})
right_wall_pose = geometry_msgs.msg.PoseStamped()
right_wall_pose.header.frame_id = '/staubli_rx60l_link1'
right_wall_dimensions = [0.92, 1.22, 0.05]
right_wall_pose.pose.position = geometry_msgs.msg.Point(**{'x': -0.6,
'y': 0.68,
'z': -0.31})
right_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': -0.50728105048,
'y': 0.487102614313,
'z': 0.482034934632,
'w': 0.522531626553})
self.scene.add_box("left_wall", left_wall_pose, wall_dimensions)
self.scene.add_box("right_wall", right_wall_pose, wall_dimensions)
self.scene.add_box("back_wall", back_wall_pose, wall_dimensions)
def add_obstacles(self):
self.add_table()
self.add_walls()
class WorldManager:
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.NEW_MODEL_REC = True
self.scene = ExtendedPlanningSceneInterface()
self.robot = moveit_commander.RobotCommander()
self.model_manager = ModelRecManager(self.NEW_MODEL_REC)
self.planning_scene_service_proxy = rospy.ServiceProxy("/get_planning_scene", moveit_msgs.srv.GetPlanningScene)
self._run_recognition_as = actionlib.SimpleActionServer("recognize_objects_action",
graspit_msgs.msg.RunObjectRecognitionAction,
execute_cb=self._run_recognition_as_cb,
auto_start=False)
self._run_recognition_as.start()
rospy.loginfo("World Manager Node is Up and Running")
def _run_recognition_as_cb(self, goal):
print("_run_recognition_as_cb")
print("about to remove_all_objects_from_planner()")
self.remove_all_objects_from_planner()
print("finished remove_all_objects_from_planner()")
self.model_manager.refresh()
print("about to add_all_objects_to_planner()")
self.add_all_objects_to_planner()
print("finished add_all_objects_to_planner()")
_result = graspit_msgs.msg.RunObjectRecognitionResult()
print("graspit_msgs.msg.RunObjectRecognitionResult()")
for model in self.model_manager.model_list:
object_info = graspit_msgs.msg.ObjectInfo(model.object_name, model.model_name, model.get_world_pose())
_result.object_info.append(object_info)
print("finished for loop")
self._run_recognition_as.set_succeeded(_result)
return []
def get_body_names_from_planner(self):
rospy.wait_for_service("/get_planning_scene", 5)
components = PlanningSceneComponents(
PlanningSceneComponents.WORLD_OBJECT_NAMES + PlanningSceneComponents.TRANSFORMS)
ps_request = moveit_msgs.srv.GetPlanningSceneRequest(components=components)
ps_response = self.planning_scene_service_proxy.call(ps_request)
body_names = [co.id for co in ps_response.scene.world.collision_objects]
return body_names
def remove_all_objects_from_planner(self):
body_names = self.get_body_names_from_planner()
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.get_body_names_from_planner()
def add_all_objects_to_planner(self):
self.add_obstacles()
for model in self.model_manager.model_list:
model_name = model.model_name.strip('/')
print "Adding " + str(model_name) + "To Moveit"
filename = file_name_dict[model_name]
if os.path.isfile(filename):
stamped_model_pose = geometry_msgs.msg.PoseStamped()
stamped_model_pose.header.frame_id = "/world"
stamped_model_pose.pose = model.get_world_pose()
self.scene.add_mesh_autoscaled(model.object_name, stamped_model_pose, filename)
else:
rospy.logwarn('File doesn\'t exist - object %s, filename %s' % (model.object_name, filename))
def add_table(self):
frame_id = "/root"
rospy.loginfo("adding table in planning frame: " + str(frame_id))
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = frame_id
table_x = rospy.get_param('/table_x', 1.45)
table_y = rospy.get_param('/table_y', .74)
table_z = rospy.get_param('/table_z', .05)
table_world_x_offset = rospy.get_param('/table_world_x_offset', -.05)
table_world_y_offset = rospy.get_param('/table_world_y_offset', -.05)
table_world_z_offset = rospy.get_param('/table_world_z_offset', -.0525)
box_pose.pose.position.x = table_world_x_offset
box_pose.pose.position.y = table_world_y_offset
box_pose.pose.position.z = table_world_z_offset
box_pose.pose.orientation.x = 0
box_pose.pose.orientation.y = 0
box_pose.pose.orientation.z = 0
box_pose.pose.orientation.w = 0
box_dimensions = (table_x, table_y, table_z)
self.scene.attach_box(world_manager.robot.get_link_names()[0], "table", box_pose, box_dimensions)
rospy.loginfo("table added")
def add_base(self):
time.sleep(1)
frame_id = "/root"
rospy.loginfo("adding table in planning frame: " + str(frame_id))
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = frame_id
base_x = rospy.get_param('/base_x', 0.2)
base_y = rospy.get_param('/base_y', 0.15)
base_z = rospy.get_param('/base_z', 0.01)
base_robot_x_offset = rospy.get_param('/base_robot_x_offset', 0)
base_robot_y_offset = rospy.get_param('/base_robot_y_offset', 0)
base_robot_z_offset = rospy.get_param('/base_robot_z_offset', -0.02)
box_pose.pose.position.x = base_robot_x_offset
box_pose.pose.position.y = base_robot_y_offset
box_pose.pose.position.z = base_robot_z_offset
box_pose.pose.orientation.x = 0
box_pose.pose.orientation.y = 0
box_pose.pose.orientation.z = 0
box_pose.pose.orientation.w = 0
box_dimensions = (base_x, base_y, base_z)
self.scene.attach_box(world_manager.robot.get_link_names()[1], "robot_base", box_pose, box_dimensions)
rospy.loginfo("base added")
def add_walls(self):
back_wall_pose = geometry_msgs.msg.PoseStamped()
back_wall_pose.header.frame_id = '/world'
wall_dimensions = [1.45, .05, .5]
back_wall_pose.pose.position = geometry_msgs.msg.Point(**{'x': .35, 'y': 0.2, 'z': 0.28})
back_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': 0,
'y': 0,
'z': 0,
'w': 0})
self.scene.add_box("back_wall", back_wall_pose, wall_dimensions)
def add_bin_wall(self, w, d, h, x, y, z, wall_name):
time.sleep(1)
bin_pose = geometry_msgs.msg.PoseStamped()
bin_pose.header.frame_id = '/root'
bin_dimensions = [w, d, h]
bin_pose.pose.position = geometry_msgs.msg.Point(**{'x': x, 'y': y, 'z': z})
bin_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': 0,
'y': 0,
'z': 0,
'w': 0})
self.scene.add_box(wall_name, bin_pose, bin_dimensions)
def add_bin(self):
# back_bin_pose = geometry_msgs.msg.PoseStamped()
# back_bin_pose.header.frame_id = '/world'
# back_bin_dimensions = [0.5, .01, .2]
# back_bin_pose.pose.position = geometry_msgs.msg.Point(**{'x': .62, 'y': -0.29, 'z': 0.16})
# back_bin_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': 0,
# 'y': 0,
# 'z': 0,
# 'w': 0})
# right_bin_pose = geometry_msgs.msg.PoseStamped()
# right_bin_pose.header.frame_id = '/world'
# right_bin_dimensions = [0.01, .25, .2]
# right_bin_pose.pose.position = geometry_msgs.msg.Point(**{'x': .36, 'y': -0.415, 'z': 0.16})
# right_bin_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{'x': 0,
# 'y': 0,
# 'z': 0,
# 'w': 0})
# self.scene.add_box("back_bin_wall", back_bin_pose, back_bin_dimensions)
# self.scene.add_box("right_bin_wall", right_bin_pose, right_bin_dimensions)
h = 0.085
z = h/2
l_1 = 0.537
l_2 = 0.254
thickness = 0.01
x = -0.1
y = -l_1 / 2
self.add_bin_wall(thickness, l_2, h,
y, x - l_2/2, z, "left_bin_wall")
self.add_bin_wall(thickness, l_2, h,
y + l_1, x - l_2/2, z, "right_bin_wall")
self.add_bin_wall(l_1, thickness, h,
y + l_1/2, x - l_2 - thickness, z, "top_bin_wall")
self.add_bin_wall(l_1, thickness, h,
y + l_1/2, x + thickness, z, "bottom_bin_wall")
self.add_bin_wall(thickness, 0.254, 0.152,
y + l_1/2, x - l_2/2, 0.076, "center_bin_wall")
def add_obstacles(self):
self.add_table()
experiment_type = rospy.get_param('~experiment_type')
if experiment_type == "block":
self.add_bin()
