def create_scene(self):
scene = PlanningSceneInterface()
rospy.sleep(1)
# clean the scene
scene.remove_world_object('box')
scene.remove_world_object('object')
#return
obj_pose = PoseStamped()
#p.header.frame_id = robot.get_planning_frame()
obj_pose.header.frame_id = 'world'
obj_pose.pose.position = self.obj_pos
obj_pose.pose.orientation = self.obj_ort
scene.add_mesh(
'grasping_object', obj_pose,
'/home/kai/Workspace/urlg_robot_ws/src/urlg_robots_gazebo/worlds/objects/pringle/optimized_poisson_texture_mapped_mesh.dae'
)
box_pose = PoseStamped()
#p.header.frame_id = robot.get_planning_frame()
box_pose.header.frame_id = 'world'
box_pose.pose.position = self.box_pos
box_pose.pose.orientation = self.box_ort
scene.add_box('box', box_pose, (0.5, 0.5, 0.5))
rospy.sleep(1)
def handle_create_moveit_scene(self, req):
scene = PlanningSceneInterface()
rospy.sleep(0.5)
#Add a box for the table.
if not self.table_box_exist:
scene.add_box('table_box', self.box_pose,
(self.box_len_x, self.box_len_y, self.box_len_z))
self.table_box_exist = True
print req.object_mesh_path
scene.add_mesh('obj_mesh', req.object_pose_world, req.object_mesh_path)
# if self.use_sim:
# scene.add_mesh('obj_mesh', req.object_pose_world, req.object_mesh_path)
# else:
# # scene.add_box('wall', self.wall_pose,
# # (self.wall_len_x, self.wall_len_y, self.wall_len_z))
# scene.add_box('obj_box', req.object_pose_world,
# (req.obj_seg.width, req.obj_seg.height, req.obj_seg.depth))
# scene.add_box('obj_box', req.object_pose_world,
# (0.02, 0.02, req.obj_seg.depth))
rospy.sleep(0.5)
response = ManageMoveitSceneResponse()
response.success = True
return response
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('paintingrobot_planningscene')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('/renov_up_level/planning_scene',
PlanningScene,
queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 设置目标位置所使用的参考坐标系
reference_frame = 'map'
stl_id = 'stl'
scene.remove_world_object(stl_id)
stl_size = [1.0, 1.0, 1.0]
stl_pose = PoseStamped()
stl_pose.header.frame_id = reference_frame
stl_pose.pose.position.x = 0.0
stl_pose.pose.position.y = 0.0
stl_pose.pose.position.z = -2.4701
stl_pose.pose.orientation.w = 1.0
scene.add_mesh(
stl_id, stl_pose,
'/home/zy/catkin_ws/src/paintingrobot/paintingrobot_underusing/painting_robot_demo/matlab/bim_document/second_scan_2.stl'
)
self.setColor(stl_id, 0.8, 0.4, 0, 1.0)
# 将场景中的颜色设置发布
while not rospy.is_shutdown():
self.sendColors()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
scene.remove_world_object("bowl")
scene.remove_world_object("punch")
scene.remove_world_object("glovebox")
rospy.sleep(2)
#reset the gripper and arm position to home
robot.set_start_state_to_current_state()
robot.set_named_target("start_glove")
robot.go()
gripper.set_start_state_to_current_state()
gripper.go()
#add scene objects
print 'adding scene objects'
scene.add_mesh("bowl", p, "8inhemi.STL")
scene.add_mesh("punch", p1, "punch.STL")
scene.add_mesh("glovebox", p2, "GloveBox.stl")
print 'attaching bowl...'
robot.attach_object("bowl")
rospy.sleep(2)
currentbowlpose = p;
gripper.set_named_target("pinch")
gripper.go()
robotstart = robot.get_current_pose()
print 'start eef pose:'
print robotstart
robotstart_quat = [robotstart.pose.orientation.x,robotstart.pose.orientation.y,robotstart.pose.orientation.z,robotstart.pose.orientation.w]
M_eef = tf.transformations.quaternion_matrix(robotstart_quat)
class GraspRose(smach.State):
def __init__(self):
smach.State.__init__(self,
outcomes=['succeeded', 'failed'],
input_keys=['active_arm'],
output_keys=['reset_rotation'])
# initialize tf listener
self.listener = tf.TransformListener()
### Create a handle for the Move Group Commander
self.mgc_left = MoveGroupCommander("arm_left")
self.mgc_right = MoveGroupCommander("arm_right")
### Create a handle for the Planning Scene Interface
self.psi = PlanningSceneInterface()
self.eef_step = 0.01
self.jump_threshold = 2
rospy.sleep(1)
def execute(self, userdata):
#rospy.loginfo("Grasping rose...")
#sss.wait_for_input()
if not self.plan_and_execute(userdata):
userdata.reset_rotation = False
return "failed"
return "succeeded"
def plan_and_execute(self, userdata):
# add table
ps = PoseStamped()
ps.header.frame_id = "table_top"
ps.pose.position.x = -0.05
ps.pose.position.z = 0.05
ps.pose.orientation.w = 1
filename = rospkg.RosPack().get_path(
"hmi_manipulation") + "/files/hmi_table.stl"
self.psi.add_mesh("table", ps, filename)
### Set next (virtual) start state
start_state = RobotState()
(pre_grasp_config,
error_code) = sss.compose_trajectory("arm_" + userdata.active_arm,
"pre_grasp")
if error_code != 0:
rospy.logerr("unable to parse pre_grasp configuration")
return False
start_state.joint_state.name = pre_grasp_config.joint_names
start_state.joint_state.position = pre_grasp_config.points[0].positions
start_state.is_diff = True
if userdata.active_arm == "left":
self.mgc_left.set_start_state(start_state)
elif userdata.active_arm == "right":
self.mgc_right.set_start_state(start_state)
else:
rospy.logerr("invalid arm_active")
return False
### Plan Approach
approach_pose_offset = PoseStamped()
approach_pose_offset.header.frame_id = "current_rose"
approach_pose_offset.header.stamp = rospy.Time(0)
approach_pose_offset.pose.position.x = -0.12
approach_pose_offset.pose.orientation.w = 1
try:
approach_pose = self.listener.transformPose(
"odom_combined", approach_pose_offset)
except Exception, e:
rospy.logerr("could not transform pose. Exception: %s", str(e))
return False
if userdata.active_arm == "left":
(traj_approach,
frac_approach) = self.mgc_left.compute_cartesian_path(
[approach_pose.pose], self.eef_step, self.jump_threshold,
True)
elif userdata.active_arm == "right":
(traj_approach,
frac_approach) = self.mgc_right.compute_cartesian_path(
[approach_pose.pose], self.eef_step, self.jump_threshold,
True)
else:
rospy.logerr("invalid arm_active")
return False
traj_approach = self.smooth_cartesian_path(traj_approach)
if not (frac_approach == 1.0):
rospy.logerr("Unable to plan approach trajectory")
#sss.say(["no approach trajectory: skipping rose"], False)
return False
### Set next (virtual) start state
traj_approach_endpoint = traj_approach.joint_trajectory.points[-1]
start_state = RobotState()
start_state.joint_state.name = traj_approach.joint_trajectory.joint_names
start_state.joint_state.position = traj_approach_endpoint.positions
start_state.is_diff = True
if userdata.active_arm == "left":
self.mgc_left.set_start_state(start_state)
elif userdata.active_arm == "right":
self.mgc_right.set_start_state(start_state)
else:
rospy.logerr("invalid arm_active")
return False
### Plan Grasp
grasp_pose_offset = PoseStamped()
grasp_pose_offset.header.frame_id = "current_rose"
grasp_pose_offset.header.stamp = rospy.Time(0)
grasp_pose_offset.pose.orientation.w = 1
grasp_pose = self.listener.transformPose("odom_combined",
grasp_pose_offset)
if userdata.active_arm == "left":
(traj_grasp, frac_grasp) = self.mgc_left.compute_cartesian_path(
[grasp_pose.pose], self.eef_step, self.jump_threshold, True)
elif userdata.active_arm == "right":
(traj_grasp, frac_grasp) = self.mgc_right.compute_cartesian_path(
[grasp_pose.pose], self.eef_step, self.jump_threshold, True)
else:
rospy.logerr("invalid arm_active")
return False
traj_grasp = self.smooth_cartesian_path(traj_grasp)
if not (frac_grasp == 1.0):
rospy.logerr("Unable to plan grasp trajectory")
#sss.say(["no grasp trajectory: skipping rose"], False)
return False
### Set next (virtual) start state
traj_grasp_endpoint = traj_grasp.joint_trajectory.points[-1]
start_state = RobotState()
start_state.joint_state.name = traj_grasp.joint_trajectory.joint_names
start_state.joint_state.position = traj_grasp_endpoint.positions
start_state.is_diff = True
if userdata.active_arm == "left":
self.mgc_left.set_start_state(start_state)
elif userdata.active_arm == "right":
self.mgc_right.set_start_state(start_state)
else:
rospy.logerr("invalid arm_active")
return False
### Plan Lift
lift_pose_offset = PoseStamped()
lift_pose_offset.header.frame_id = "current_rose"
lift_pose_offset.header.stamp = rospy.Time(0)
if userdata.active_arm == "left":
lift_pose_offset.pose.position.z = -0.2 #-0.3#-0.12
elif userdata.active_arm == "right":
lift_pose_offset.pose.position.z = 0.2 #0.3#0.12
else:
rospy.logerr("invalid active_arm: %s", userdata.active_arm)
sys.exit()
lift_pose_offset.pose.orientation.w = 1
lift_pose = self.listener.transformPose("odom_combined",
lift_pose_offset)
if userdata.active_arm == "left":
(traj_lift, frac_lift) = self.mgc_left.compute_cartesian_path(
[lift_pose.pose], self.eef_step, self.jump_threshold, True)
elif userdata.active_arm == "right":
(traj_lift, frac_lift) = self.mgc_right.compute_cartesian_path(
[lift_pose.pose], self.eef_step, self.jump_threshold, True)
else:
rospy.logerr("invalid arm_active")
return False
traj_lift = self.smooth_cartesian_path(traj_lift)
if not (frac_lift == 1.0):
rospy.logerr("Unable to plan lift trajectory")
#sss.say(["no lift trajectory: skipping rose"], False)
return False
"""
### Set next (virtual) start state
traj_lift_endpoint = traj_lift.joint_trajectory.points[-1]
start_state = RobotState()
start_state.joint_state.name = traj_lift.joint_trajectory.joint_names
start_state.joint_state.position = traj_lift_endpoint.positions
rose_primitive = SolidPrimitive()
rose_primitive.type = 3 #CYLINDER
rose_height = 0.4
rose_radius = 0.05
rose_primitive.dimensions.append(rose_height)
rose_primitive.dimensions.append(rose_radius)
rose_pose = Pose()
rose_pose.orientation.w = 1.0
rose_collision = CollisionObject()
rose_collision.header.frame_id = "gripper_"+userdata.active_arm+"_grasp_link"
rose_collision.id = "current_rose"
rose_collision.primitives.append(rose_primitive)
rose_collision.primitive_poses.append(rose_pose)
rose_collision.operation = 0 #ADD
rose_attached = AttachedCollisionObject()
rose_attached.link_name = "gripper_"+userdata.active_arm+"_grasp_link"
rose_attached.object = rose_collision
rose_attached.touch_links = ["gripper_"+userdata.active_arm+"_base_link", "gripper_"+userdata.active_arm+"_camera_link", "gripper_"+userdata.active_arm+"_finger_1_link", "gripper_"+userdata.active_arm+"_finger_2_link", "gripper_"+userdata.active_arm+"_grasp_link", "gripper_"+userdata.active_arm+"_palm_link"]
start_state.attached_collision_objects.append(rose_attached)
start_state.is_diff = True
if userdata.active_arm == "left":
self.mgc_left.set_start_state(start_state)
elif userdata.active_arm == "right":
self.mgc_right.set_start_state(start_state)
else:
rospy.logerr("invalid arm_active")
return False
#Plan retreat
pre_grasp_config = smi.get_goal_from_server("arm_"+userdata.active_arm, "pre_grasp")
#print pre_grasp_config
if pre_grasp_config == None:
rospy.logerr("GoalConfig not found on ParameterServer")
#sss.say(["GoalConfig not found on ParameterServer"], False)
return False
if userdata.active_arm == "left":
self.mgc_left.set_planner_id("RRTkConfigDefault")
traj_pre_grasp = self.mgc_left.plan(pre_grasp_config)
elif userdata.active_arm == "right":
self.mgc_right.set_planner_id("RRTkConfigDefault")
traj_pre_grasp = self.mgc_right.plan(pre_grasp_config)
else:
rospy.logerr("invalid arm_active")
return False
print traj_pre_grasp
if traj_pre_grasp == None:
rospy.logerr("Unable to plan pre_grasp trajectory")
#sss.say(["no pre_grasp trajectory: skipping rose"], False)
return False
"""
#if not (frac_approach == 1.0 and frac_grasp == 1.0 and frac_lift == 1.0 and not traj_pre_grasp == None):
if not (frac_approach == 1.0 and frac_grasp == 1.0
and frac_lift == 1.0):
rospy.logerr("Unable to plan whole grasping trajectory")
#sss.say(["skipping rose"], False)
return False
else:
#sss.say(["grasping rose"], False)
# fix trajectories to stop at the end
traj_approach.joint_trajectory.points[-1].velocities = [0] * 7
traj_grasp.joint_trajectory.points[-1].velocities = [0] * 7
traj_lift.joint_trajectory.points[-1].velocities = [0] * 7
# fix trajectories to be slower
speed_factor = 1
for i in range(len(traj_approach.joint_trajectory.points)):
traj_approach.joint_trajectory.points[
i].time_from_start *= speed_factor
for i in range(len(traj_grasp.joint_trajectory.points)):
traj_grasp.joint_trajectory.points[
i].time_from_start *= speed_factor
for i in range(len(traj_lift.joint_trajectory.points)):
traj_lift.joint_trajectory.points[
i].time_from_start *= speed_factor
### execute
#sss.wait_for_input()
sss.move("arm_" + userdata.active_arm, "pre_grasp")
#sss.wait_for_input()
rospy.loginfo("approach")
if userdata.active_arm == "left":
self.mgc_left.execute(traj_approach)
#handle_gripper = sss.move("gripper_" + userdata.active_arm, "open")
move_gripper("gripper_" + userdata.active_arm, "open")
#sss.wait_for_input()
rospy.loginfo("grasp")
self.mgc_left.execute(traj_grasp)
#sss.wait_for_input()
#sss.move("gripper_" + userdata.active_arm, "close")
move_gripper("gripper_" + userdata.active_arm, "close")
rospy.loginfo("lift")
self.mgc_left.execute(traj_lift)
#sss.wait_for_input()
#self.mgc_left.execute(traj_pre_grasp)
#rospy.sleep(1)
sss.move("base", "middle", mode="linear", blocking=False)
#rospy.sleep(0.5) #wait for base to move away from table
handle_arm = sss.move("arm_" + userdata.active_arm, "retreat")
elif userdata.active_arm == "right":
self.mgc_right.execute(traj_approach)
#sss.move("gripper_" + userdata.active_arm, "open")
move_gripper("gripper_" + userdata.active_arm, "open")
#sss.wait_for_input()
rospy.loginfo("grasp")
self.mgc_right.execute(traj_grasp)
#sss.wait_for_input()
#sss.move("gripper_" + userdata.active_arm, "close")
move_gripper("gripper_" + userdata.active_arm, "close")
rospy.loginfo("lift")
self.mgc_right.execute(traj_lift)
#sss.wait_for_input()
#self.mgc_right.execute(traj_pre_grasp)
#rospy.sleep(1)
sss.move("base", "middle", mode="linear", blocking=False)
#rospy.sleep(0.5) #wait for base to move away from table
handle_arm = sss.move("arm_" + userdata.active_arm, "retreat")
else:
rospy.logerr("invalid arm_active")
return False
#if handle_arm.get_error_code():
# #sss.say(["script server error"])
# rospy.logerr("script server error")
# sss.set_light("light_base","red")
#sss.wait_for_input()
return True
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('SceneSetup')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Set the reference frame for pose targets
reference_frame = 'rack1'
table_id = 'table'
bowl_id = 'bowl'
pitcher_id = 'pitcher'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(bowl_id)
scene.remove_world_object(pitcher_id)
# Set the height of the table off the ground
table_ground = 0.5762625
# Set the length, width and height of the table and boxes
table_size = [1.0128, 0.481, 0.0238125]
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0
table_pose.pose.position.y = 0.847725
table_pose.pose.position.z = table_ground
# Set the height of the bowl
bowl_ground = 0.57816875
bowl_pose = PoseStamped()
bowl_pose.header.frame_id = reference_frame
bowl_pose.pose.position.x = 0
bowl_pose.pose.position.y = 0.847725
bowl_pose.pose.position.z = bowl_ground
# Set the height of the pitcher
pitcher_ground = 0.57816875
pitcher_pose = PoseStamped()
pitcher_pose.header.frame_id = reference_frame
pitcher_pose.pose.position.x = 0.4
pitcher_pose.pose.position.y = 0.847725
pitcher_pose.pose.position.z = pitcher_ground
pitcher_pose.pose.orientation.w = -0.5
pitcher_pose.pose.orientation.z = 0.707
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0.4, 0, 1.0)
self.setColor(bowl_id, 0, 0.4, 0.8, 1.0)
self.setColor(pitcher_id, 0, 0.4, 0.8, 1.0)
self.sendColors()
scene.add_box(table_id, table_pose, table_size)
scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl')
#scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
####################### ADD RIGHT SHOE TO THE SCENE ###############################
scene.remove_attached_object('r_eef', 'right_shoe')
rospy.sleep(1)
pose.pose.position.x = 0.475
pose.pose.position.y = -0.1500
pose.pose.position.z = 1.035
pose.pose.orientation.w = 0.99144
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0.13053
pose.header.frame_id = robot.get_planning_frame()
pose.header.stamp = rospy.Time.now()
path = roslib.packages.get_pkg_dir('hermes_virtual_robot')+'/common/files/stl/right_shoe.stl'
scene.add_mesh("right_shoe", pose, path)
rospy.sleep(1)
####################### PREGRASP RIGHT HAND ###############################
req = rospy.ServiceProxy('grasp_hand', GraspHand)
res = req(2,1) # 2 -> Right _hand 1-> PreGrasp
res = req(1,1) # 2 -> Right _hand 1-> PreGrasp
rospy.sleep(1)
####################### POSICION DE GRASP ###############################
pose.pose.position.x = 0.4508
pose.pose.position.y = -0.3605
pose.pose.position.z = 1.1423
pose.pose.orientation.w = 0.6812
pose.pose.orientation.x = -0.21744
pose.pose.orientation.y = 0.65471
pose.pose.orientation.z = 0.24387
pose.header.frame_id = robot.get_planning_frame()
def main():
rospy.init_node('duaro')
botharms = MoveGroupCommander("botharms")
upper = MoveGroupCommander("upper_arm")
lower = MoveGroupCommander("lower_arm")
duaro = RobotCommander()
# Add Object to Planning Scene
rospy.loginfo("Planning Scene Settings")
scene = PlanningSceneInterface()
rospy.sleep(2) # Waiting for PlanningSceneInterface
scene.remove_world_object()
current_robot = duaro.get_current_state()
current_joint = current_robot.joint_state.position
rospack = rospkg.RosPack()
resourcepath = rospack.get_path(
'khi_duaro_onigiri_system') + "/config/meshes/"
#conveyer
box_pose = PoseStamped()
box_pose.header.frame_id = botharms.get_planning_frame()
rot_o = 180.0 / 180.0 * math.pi
rot_a = 0.0 / 180.0 * math.pi
rot_t = 0.0 / 180.0 * math.pi
q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx')
#setting origin of conveyer
box_pose.pose.position.x = 0.2153
box_pose.pose.position.y = 0.32454
box_pose.pose.position.z = 0.0
box_pose.pose.orientation.x = q[0]
box_pose.pose.orientation.y = q[1]
box_pose.pose.orientation.z = q[2]
box_pose.pose.orientation.w = q[3]
meshpath = resourcepath + "conveyer.stl"
scene.add_mesh('conveyer', box_pose, meshpath, (1, 1, 1))
rospy.sleep(3) # Waiting for setting conveyer
#stand
rot_o = 0.0 / 180.0 * math.pi
rot_a = 0.0 / 180.0 * math.pi
rot_t = 0.0 / 180.0 * math.pi
q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx')
#setting origin of stand
box_pose.pose.position.x = -0.385
box_pose.pose.position.y = 0.2342
box_pose.pose.position.z = 0.0
box_pose.pose.orientation.x = q[0]
box_pose.pose.orientation.y = q[1]
box_pose.pose.orientation.z = q[2]
box_pose.pose.orientation.w = q[3]
meshpath = resourcepath + "stand.stl"
scene.add_mesh('stand', box_pose, meshpath, (1, 1, 1))
rospy.sleep(3) # Waiting for setting stand
rospy.loginfo("Planning Scene Settings Finish")
#tray
rot_o = 0.0 / 180.0 * math.pi
rot_a = 0.0 / 180.0 * math.pi
rot_t = 0.0 / 180.0 * math.pi
q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx')
#setting origin of tray
box_pose.pose.position.x = -0.58952
box_pose.pose.position.y = -0.0613
box_pose.pose.position.z = 0.6375
box_pose.pose.orientation.x = q[0]
box_pose.pose.orientation.y = q[1]
box_pose.pose.orientation.z = q[2]
box_pose.pose.orientation.w = q[3]
meshpath = resourcepath + "tray_nooffset.stl"
scene.add_mesh('tray', box_pose, meshpath, (1, 1, 1))
rospy.loginfo("Planning Scene Settings Finish")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.7
p.pose.position.y = -0.4
p.pose.position.z = 0.85
p.pose.orientation.w = 1.57
scene.add_box("block1", p, (0.044, 0.044, 0.044))
p.pose.position.y = -0.2
p.pose.position.z = 0.175
scene.add_box("block2", p, (0.044, 0.044, 0.044))
p.pose.position.x = 0.6
p.pose.position.y = -0.7
p.pose.position.z = 0.5
scene.add_box("block3", p, (0.044, 0.044, 0.044))
p.pose.position.x = 1
p.pose.position.y = -0.7
p.pose.position.z = 0.5
scene.add_box("block4", p, (0.044, 0.044, 0.044))
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = 0
scene.add_mesh("table", p, "table_scaled.stl")
scene.add_mesh("bowl", p, "bowl_scaled.stl")
scene.add_mesh("box", p, "box_scaled.stl")
rospy.sleep(1)
# pick an object
robot.right_arm.pick("block2")
rospy.spin()
roscpp_shutdown()
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# add a box there
scene.add_box("ground", p, (3, 3, 0.02))
# access some meshes
rospack = rospkg.RosPack()
resourcepath = rospack.get_path('regrasping_app')+"/../resources/"
# modify the pose
p.pose.position.x = 0.7
p.pose.position.y = 0.7
p.pose.position.z = 0.0
# add the cup
scene.add_mesh("cup",p,resourcepath+'objects/cup.dae')
# modify the pose
p.pose.position.x = 0.72
p.pose.position.z = 0.05
# add the pen
scene.add_mesh("pen",p,resourcepath+'objects/pen.dae')
rospy.sleep(1)
# print the existing groups
robot = RobotCommander()
print "Available groups: ",robot.get_group_names()
# setup the arm group and its planner
arm = MoveGroupCommander("arm")
arm.set_start_state_to_current_state()
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)
def main(argv):
#This should become dynamic, e.g. rosparam
offset = [-0.450, -0.450, 0.0]
UDP_IP = "127.0.0.1"
UDP_PORT = 9000
rospy.init_node("Collision_adder")
transform = tf.TransformListener()
rospack = rospkg.RosPack()
#while not t.canTransform("shoulder_link", "base_link", rospy.Time.now()):
# print "."
mesh_path = rospack.get_path("ur_description") + "/meshes/ur5/collision/"
scene = PlanningSceneInterface()
robot = RobotCommander()
rospy.sleep(2)
sock = socket.socket(
socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
sock.bind((UDP_IP, UDP_PORT))
#I assume that the base can not move, e.g. is mounted in a fixed location.
quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = offset[0]
p.pose.position.y = offset[1]
p.pose.position.z = offset[2]
p.pose.orientation.x = quaternion[0]
p.pose.orientation.y = quaternion[1]
p.pose.orientation.z = quaternion[2]
p.pose.orientation.w = quaternion[3]
scene.add_mesh("base", p, mesh_path + "/base.stl")
#scene.is_diff = True
while not rospy.is_shutdown():
data, addr = sock.recvfrom(2024) # buffer size is 1024 bytes
print "received message:", data
jointstates = data.split(",")
#t = transform.getLatestCommonTime("/shoulder_link", "/base_link")
#shoulder_pose, shoulder_quaternion = transform.lookupTransform("base_link", "shoulder_link", t)
shoulderstate = jointstates[0].split(";")
shoulder = PoseStamped()
shoulder.header.frame_id = robot.get_planning_frame()
shoulder.pose.position.x = offset[0] + float(shoulderstate[1])
shoulder.pose.position.y = offset[1] + float(shoulderstate[2])
shoulder.pose.position.z = offset[2] + float(shoulderstate[3])
shoulder.pose.orientation.x = float(shoulderstate[4])
shoulder.pose.orientation.y = float(shoulderstate[5])
shoulder.pose.orientation.z = float(shoulderstate[6])
shoulder.pose.orientation.w = float(shoulderstate[7])
scene.add_mesh(shoulderstate[0], shoulder,
mesh_path + shoulderstate[0] + ".stl")
#t = transform.getLatestCommonTime("/upper_arm_link", "/base_link")
#upper_arm_pose, upper_arm_quaternion = transform.lookupTransform("base_link", "upper_arm_link", t)
upperarmstate = jointstates[1].split(";")
upper_arm = PoseStamped()
upper_arm.header.frame_id = robot.get_planning_frame()
upper_arm.pose.position.x = offset[0] + float(upperarmstate[1])
upper_arm.pose.position.y = offset[1] + float(upperarmstate[2])
upper_arm.pose.position.z = offset[2] + float(upperarmstate[3])
upper_arm.pose.orientation.x = float(upperarmstate[4])
upper_arm.pose.orientation.y = float(upperarmstate[5])
upper_arm.pose.orientation.z = float(upperarmstate[6])
upper_arm.pose.orientation.w = float(upperarmstate[7])
scene.add_mesh(upperarmstate[0], upper_arm,
mesh_path + upperarmstate[0] + ".stl")
#t = transform.getLatestCommonTime("/forearm_link", "/base_link")
#fore_arm_pose, fore_arm_quaternion = transform.lookupTransform("base_link", "forearm_link", t)
forearmstate = jointstates[2].split(";")
fore_arm = PoseStamped()
fore_arm.header.frame_id = robot.get_planning_frame()
fore_arm.pose.position.x = offset[0] + float(forearmstate[1])
fore_arm.pose.position.y = offset[1] + float(forearmstate[2])
fore_arm.pose.position.z = offset[2] + float(forearmstate[3])
fore_arm.pose.orientation.x = float(forearmstate[4])
fore_arm.pose.orientation.y = float(forearmstate[5])
fore_arm.pose.orientation.z = float(forearmstate[6])
fore_arm.pose.orientation.w = float(forearmstate[7])
scene.add_mesh(forearmstate[0], fore_arm,
mesh_path + forearmstate[0] + ".stl")
#t = transform.getLatestCommonTime("/wrist_1_link", "/base_link")
#wrist1_pose, wrist1_quaternion = transform.lookupTransform("base_link", "wrist_1_link", t)
wrist1state = jointstates[3].split(";")
wrist1 = PoseStamped()
wrist1.header.frame_id = robot.get_planning_frame()
wrist1.pose.position.x = offset[0] + float(wrist1state[1])
wrist1.pose.position.y = offset[1] + float(wrist1state[2])
wrist1.pose.position.z = offset[2] + float(wrist1state[3])
wrist1.pose.orientation.x = float(wrist1state[4])
wrist1.pose.orientation.y = float(wrist1state[5])
wrist1.pose.orientation.z = float(wrist1state[6])
wrist1.pose.orientation.w = float(wrist1state[7])
scene.add_mesh(wrist1state[0], wrist1,
mesh_path + wrist1state[0] + ".stl")
#t = transform.getLatestCommonTime("/wrist_2_link", "/base_link")
#wrist2_pose, wrist2_quaternion = transform.lookupTransform("base_link", "wrist_2_link", t)
wrist2state = jointstates[4].split(";")
wrist2 = PoseStamped()
wrist2.header.frame_id = robot.get_planning_frame()
wrist2.pose.position.x = offset[0] + float(wrist2state[1])
wrist2.pose.position.y = offset[1] + float(wrist2state[2])
wrist2.pose.position.z = offset[2] + float(wrist2state[3])
wrist2.pose.orientation.x = float(wrist2state[4])
wrist2.pose.orientation.y = float(wrist2state[5])
wrist2.pose.orientation.z = float(wrist2state[6])
wrist2.pose.orientation.w = float(wrist2state[7])
scene.add_mesh(wrist2state[0], wrist2,
mesh_path + wrist2state[0] + ".stl")
#t = transform.getLatestCommonTime("/wrist_3_link", "/base_link")
#wrist3_pose, wrist3_quaternion = transform.lookupTransform("base_link", "wrist_3_link", t)
wrist3state = jointstates[5].split(";")
wrist3 = PoseStamped()
wrist3.header.frame_id = robot.get_planning_frame()
wrist3.pose.position.x = offset[0] + float(wrist3state[1])
wrist3.pose.position.y = offset[1] + float(wrist3state[2])
wrist3.pose.position.z = offset[2] + float(wrist3state[3])
wrist3.pose.orientation.x = float(wrist3state[4])
wrist3.pose.orientation.y = float(wrist3state[5])
wrist3.pose.orientation.z = float(wrist3state[6])
wrist3.pose.orientation.w = float(wrist3state[7])
scene.add_mesh(wrist3state[0], wrist3,
mesh_path + wrist3state[0] + ".stl")
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# add a box there
scene.add_box("ground", p, (3, 3, 0.02))
# access some meshes
rospack = rospkg.RosPack()
resourcepath = rospack.get_path('regrasping_app') + "/../resources/"
# modify the pose
p.pose.position.x = 0.7
p.pose.position.y = 0.7
p.pose.position.z = 0.0
# add the cup
scene.add_mesh("cup", p, resourcepath + 'objects/cup.dae')
# modify the pose
p.pose.position.x = 0.72
p.pose.position.z = 0.05
# add the pen
scene.add_mesh("pen", p, resourcepath + 'objects/pen.dae')
rospy.sleep(1)
# print the existing groups
robot = RobotCommander()
print "Available groups: ", robot.get_group_names()
# setup the arm group and its planner
arm = MoveGroupCommander("arm")
arm.set_start_state_to_current_state()
robot.set_named_target("home")
robot.go()
startpose = PoseStamped()
startpose.header.frame_id = 'world'
startpose.header.stamp = rospy.Time.now()
startpose = robot.get_current_pose()
startpose.pose.position.x -= 0.3
robot.set_pose_target(startpose)
robot.go()
# gripper.set_start_state_to_current_state()
# gripper.set_named_target("default")
# gripper.go()
#add scene objects
print 'adding scene objects'
scene.add_mesh("bowl", bowl_pose, "8inhemi.STL")
scene.add_mesh("punch", punch_pose, "punch.STL")
rospy.sleep(1)
# gripper.set_named_target("pinch")
# gripper.go()
rad = 4
rad = inchtometer(rad)
grasp_start = PoseStamped()
grasp_start.header.frame_id = 'world'
grasp_start.header.stamp = rospy.Time.now()
grasp_start.pose.position.x = 0
grasp_start.pose.position.y = -0.5+rad
grasp_start.pose.position.z = 0.4
grasp_start.pose.orientation.x = 0
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)
class WorldManager:
def __init__(self, server):
self.server = server
moveit_commander.roscpp_initialize(sys.argv)
self.planning_scene_topic = rospy.get_param("planning_scene_topic")
self.run_recognition_topic = rospy.get_param("run_recognition_topic")
self.detected_model_frame_id = rospy.get_param(
"detected_model_frame_id")
self.scene = PlanningSceneInterface()
self.robot = moveit_commander.RobotCommander()
self.model_manager = ModelRecManager()
self.planning_scene_service_proxy = rospy.ServiceProxy(
self.planning_scene_topic, moveit_msgs.srv.GetPlanningScene)
self._run_recognition_as = actionlib.SimpleActionServer(
self.run_recognition_topic,
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)
position = model.get_world_pose().position
position = Point(position.x, position.y, position.z)
make6DofMarker(
fixed=False,
frame=model.model_name,
interaction_mode=InteractiveMarkerControl.MOVE_ROTATE_3D,
position=position,
show_6dof=True)
print("finished for loop")
server.applyChanges()
self._run_recognition_as.set_succeeded(_result)
return []
def get_body_names_from_planner(self):
rospy.wait_for_service(self.planning_scene_topic, 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 = self.detected_model_frame_id
stamped_model_pose.pose = model.get_world_pose()
self.scene.add_mesh(model.object_name, stamped_model_pose,
filename)
else:
rospy.logwarn('File doesn\'t exist - object %s, filename %s' %
(model.object_name, filename))
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)
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)
return
def add_obstacles(self):
self.add_walls()
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)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('MotionSequence')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying left wam finger poses
self.left_wam_finger_1_pub = rospy.Publisher('left_wam_finger_1', PoseStamped)
self.left_wam_finger_2_pub = rospy.Publisher('left_wam_finger_2', PoseStamped)
self.left_wam_finger_3_pub = rospy.Publisher('left_wam_finger_3', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Define move group comander for each moveit group
left_wam = moveit_commander.MoveGroupCommander('left_wam')
right_wam = moveit_commander.MoveGroupCommander('right_wam')
left_wam_finger_1 = moveit_commander.MoveGroupCommander('left_wam_finger_1')
left_wam_finger_2 = moveit_commander.MoveGroupCommander('left_wam_finger_2')
left_wam_finger_3 = moveit_commander.MoveGroupCommander('left_wam_finger_3')
right_wam_finger_1 = moveit_commander.MoveGroupCommander('right_wam_finger_1')
right_wam_finger_2 = moveit_commander.MoveGroupCommander('right_wam_finger_2')
right_wam_finger_3 = moveit_commander.MoveGroupCommander('right_wam_finger_3')
left_wam.set_planner_id("PRMstarkConfigDefault")
right_wam.set_planner_id("PRMstarkConfigDefault")
#left_wam_finger_1.set_planner_id("RRTstarkConfigDefault")
#left_wam_finger_2.set_planner_id("RRTstarkConfigDefault")
#left_wam_finger_3.set_planner_id("RRTstarkConfigDefault")
#right_wam_finger_1.set_planner_id("RRTstarkConfigDefault")
#right_wam_finger_2.set_planner_id("RRTstarkConfigDefault")
#right_wam_finger_3.set_planner_id("RRTstarkConfigDefault")
# Get the name of the end-effector link
left_end_effector_link = left_wam.get_end_effector_link()
right_end_effector_link = right_wam.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link of left wam is: " + str(left_end_effector_link))
rospy.loginfo("The end effector link of right wam is: " + str(right_end_effector_link))
# Allow some leeway in position (meters) and orientation (radians)
right_wam.set_goal_position_tolerance(0.01)
right_wam.set_goal_orientation_tolerance(0.05)
left_wam.set_goal_position_tolerance(0.01)
left_wam.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_wam.allow_replanning(True)
left_wam.allow_replanning(True)
# Allow 5 seconds per planning attempt
right_wam.set_planning_time(15)
left_wam.set_planning_time(25)
# Allow replanning to increase the odds of a solution
right_wam.allow_replanning(True)
left_wam.allow_replanning(True)
# Set the reference frame for wam arms
left_wam.set_pose_reference_frame(REFERENCE_FRAME)
right_wam.set_pose_reference_frame(REFERENCE_FRAME)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
bowl_id = 'bowl'
pitcher_id = 'pitcher'
spoon_id = 'spoon'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(bowl_id)
scene.remove_world_object(pitcher_id)
scene.remove_world_object(spoon_id)
# Remove leftover objects from a previous run
scene.remove_attached_object(right_end_effector_link, 'spoon')
#right_wam.set_named_target('right_wam_start')
#right_wam.go()
#rospy.sleep(2)
# Closing the hand first
# Closing the hand
#right_wam_finger_1.set_named_target("right_wam_finger_1_grasp")
#right_wam_finger_2.set_named_target("right_wam_finger_2_grasp")
#right_wam_finger_3.set_named_target("right_wam_finger_3_grasp")
#right_wam_finger_1.execute(right_wam_finger_1.plan())
#rospy.sleep(5)
#right_wam_finger_2.execute(right_wam_finger_2.plan())
#rospy.sleep(5)
#right_wam_finger_3.execute(right_wam_finger_3.plan())
#rospy.sleep(5)
# Create a pose for the tool relative to the end-effector
p = PoseStamped()
p.header.frame_id = right_end_effector_link
# Place the end of the object within the grasp of the gripper
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.02
# Align the object with the gripper (straight out)
p.pose.orientation.x = -0.5
p.pose.orientation.y = 0.5
p.pose.orientation.z = -0.5
p.pose.orientation.w = 0.5
# Attach the tool to the end-effector
# Set the height of the table off the ground
table_ground = 0.5762625
# Set the length, width and height of the table and boxes
table_size = [0.90128, 0.381, 0.0238125]
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0
table_pose.pose.position.y = 0.847725
table_pose.pose.position.z = table_ground
scene.add_box(table_id, table_pose, table_size)
# Set the height of the bowl
bowl_ground = 0.57816875
bowl_pose = PoseStamped()
bowl_pose.header.frame_id = REFERENCE_FRAME
bowl_pose.pose.position.x = 0.015
bowl_pose.pose.position.y = 0.847725
bowl_pose.pose.position.z = bowl_ground
scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl')
# Set the height of the pitcher
#pitcher_ground = 0.57816875
#pitcher_pose = PoseStamped()
#pitcher_pose.header.frame_id = REFERENCE_FRAME
#pitcher_pose.pose.position.x = 0.25
#pitcher_pose.pose.position.y = 0.847725
#pitcher_pose.pose.position.z = pitcher_ground
#pitcher_pose.pose.orientation.w = -0.5
#pitcher_pose.pose.orientation.z = 0.707
#scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl')
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0.4, 0, 1.0)
self.setColor(bowl_id, 0, 0.4, 0.8, 1.0)
#self.setColor(pitcher_id, 0.9, 0.9, 0, 1.0)
self.sendColors()
rospy.sleep(2)
start = input("Start left_wam planning ? ")
# Set the support surface name to the table object
#left_wam.set_support_surface_name(table_id)
#right_wam.set_support_surface_name(table_id)
# Set the target pose.
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.40363476287
target_pose.pose.position.y = 0.847725
target_pose.pose.position.z = 0.721472317843
target_pose.pose.orientation.x = 0.707
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = -0.707
target_pose.pose.orientation.w = 0
# Set the start state to the current state
left_wam.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
left_wam.set_pose_target(target_pose, left_end_effector_link)
left_wam.execute(left_wam.plan())
left_wam.shift_pose_target(5, 0, left_end_effector_link)
start = input("Left wam starting position ")
# Pause for a second
# Closing the hand
left_wam_finger_1.set_named_target("left_wam_finger_1_grasp")
left_wam_finger_2.set_named_target("left_wam_finger_2_grasp")
left_wam_finger_3.set_named_target("left_wam_finger_3_grasp")
left_wam_finger_1.execute(left_wam_finger_1.plan())
#rospy.sleep(3)
left_wam_finger_2.execute(left_wam_finger_2.plan())
#rospy.sleep(3)
left_wam_finger_3.execute(left_wam_finger_3.plan())
#rospy.sleep(3)
start = input("Left wam hand closing ")
end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link).pose)
intermidiate_pose = deepcopy(end_pose)
intermidiate_pose.position.z = intermidiate_pose.position.z + 0.05
plan = self.StraightPath(end_pose, intermidiate_pose, left_wam)
left_wam.set_start_state_to_current_state()
left_wam.execute(plan)
start = input("Hold up the Pitcher ")
end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link).pose)
intermidiate_pose = deepcopy(end_pose)
intermidiate_pose.position.x = intermidiate_pose.position.x - 0.1
plan = self.StraightPath(end_pose, intermidiate_pose, left_wam)
left_wam.set_start_state_to_current_state()
left_wam.execute(plan)
start = input("left_wam into pouring position ")
end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link))
back_pose = deepcopy(end_pose)
end_pose.pose.orientation.x = 0.97773401145
end_pose.pose.orientation.y = 0
end_pose.pose.orientation.z = -0.209726592658
end_pose.pose.orientation.w = 0
# Set the start state to the current state
left_wam.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
left_wam.set_pose_target(end_pose, left_end_effector_link)
left_wam.execute(left_wam.plan())
start = input("Pour the water ")
# Set the start state to the current state
left_wam.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
left_wam.set_pose_target(back_pose, left_end_effector_link)
left_wam.execute(left_wam.plan())
end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link))
end_pose.pose.position.x = end_pose.pose.position.x + 0.1
# Set the start state to the current state
left_wam.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
left_wam.set_pose_target(end_pose, left_end_effector_link)
left_wam.execute(left_wam.plan())
start = input("Left_wam back to prep position")
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = -0.600350195463908
target_pose.pose.position.y = 0.80576308041
target_pose.pose.position.z = 0.794775212132
target_pose.pose.orientation.x = 3.01203251908e-05
target_pose.pose.orientation.y = 0.705562870053
target_pose.pose.orientation.z = 4.55236739937e-05
target_pose.pose.orientation.w = 0.708647326547
start = input("Start right_wam planning ? ")
right_wam.set_start_state_to_current_state()
right_wam.set_pose_target(target_pose, right_end_effector_link)
right_wam.execute(right_wam.plan())
start = input("right_wam into position. ")
start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
intermidiate_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
intermidiate_pose.position.x = intermidiate_pose.position.x + 0.6
plan = self.StraightPath(start_pose, intermidiate_pose, right_wam)
right_wam.set_start_state_to_current_state()
right_wam.execute(plan)
rospy.sleep(3)
# Closing the hand
right_wam_finger_1.set_named_target("right_wam_finger_1_grasp")
right_wam_finger_2.set_named_target("right_wam_finger_2_grasp")
right_wam_finger_3.set_named_target("right_wam_finger_3_grasp")
right_wam_finger_1.execute(right_wam_finger_1.plan())
#rospy.sleep(3)
right_wam_finger_2.execute(right_wam_finger_2.plan())
#rospy.sleep(3)
right_wam_finger_3.execute(right_wam_finger_3.plan())
rospy.sleep(1)
scene.attach_mesh(right_end_effector_link, 'spoon', p, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/spoon.stl')
#create a circle path
circles = input("How many circles you want the wam to mix ? ")
start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
plan = self.CircularPath(start_pose, circles, right_wam)
#execute the circle path
right_wam.set_start_state_to_current_state()
right_wam.execute(plan)
pause = input("Mix the oatmeal ")
#put the right_wam back to preparation pose
end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
intermidiate_pose1 = deepcopy(end_pose)
intermidiate_pose1.position.z = intermidiate_pose1.position.z + 0.1
plan = self.StraightPath(end_pose, intermidiate_pose1, right_wam)
right_wam.set_start_state_to_current_state()
right_wam.execute(plan)
pause = input("wait for the execution of straight path ")
end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose)
intermidiate_pose2 = deepcopy(end_pose)
intermidiate_pose2.position.x = intermidiate_pose2.position.x - 0.25
plan = self.StraightPath(end_pose, intermidiate_pose2, right_wam)
right_wam.set_start_state_to_current_state()
right_wam.execute(plan)
pause = input("right_wam back into prep position ")
#left_wam.shift_pose_target(5, 0, left_end_effector_link)
#left_wam.go()
#rospy.sleep(2)
#left_wam.shift_pose_target(0, -0.05, left_end_effector_link)
#left_wam.go()
#rospy.sleep(2)
# Initialize the grasp pose to the target pose
#grasp_pose = target_pose
# Generate a list of grasps
#grasps = self.make_grasps(grasp_pose, [pitcher_id])
# Publish the grasp poses so they can be viewed in RViz
#for grasp in grasps:
# self.left_wam_finger_1_pub.publish(grasp.grasp_pose)
# rospy.sleep(0.2)
# self.left_wam_finger_2_pub.publish(grasp.grasp_pose)
# rospy.sleep(0.2)
# self.left_wam_finger_3_pub.publish(grasp.grasp_pose)
# rospy.sleep(0.2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class PlanningSceneHelper:
def __init__(self, package=None, mesh_folder_path=None):
# interface to the planning and collision scenes
self.psi = PlanningSceneInterface()
# self.scene_pub = Publisher("/collision_object", CollisionObject,
# queue_size=1, latch=True)
self.vis_pub = Publisher("/collision_scene",
MarkerArray,
queue_size=10)
self.marker_array = MarkerArray()
sleep(1.0)
if package is not None and mesh_folder_path is not None:
# Build folder path for use in load
rospack = RosPack()
self.package = package
self.mesh_folder_path = mesh_folder_path
self.package_path = rospack.get_path(package)
self.folder_path = os.path.join(self.package_path, mesh_folder_path)\
+ os.sep
else:
loginfo(
"Missing package or mesh folder path supplied to planning_scene_helper; no meshes can be added"
)
# Create dict of objects, for removing markers later
self.objects = {}
self.next_id = 1
# Loads the selected mesh file into collision scene at the desired location.
def add_mesh(self,
object_id,
frame,
filename,
visual=None,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
if self.package is None:
logwarn("No package was provided; no meshes can be loaded.")
if visual is None:
visual = filename
filename = self.folder_path + filename
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
try:
self.psi.add_mesh(object_id, stamped_pose, filename)
loginfo("Loaded " + object_id + " from" + filename +
" as collision object.")
marker = self.make_marker_stub(stamped_pose, color=color)
marker.type = Marker.MESH_RESOURCE
# marker.mesh_resource = "file:/" + self.folder_path + visual
marker.mesh_resource = "package://" + self.package + os.sep + self.mesh_folder_path + os.sep + visual
self.marker_array = self.make_new_marker_array_msg()
self.marker_array.markers.append(marker)
self.vis_pub.publish(self.marker_array)
self.objects[object_id] = marker
loginfo("Loaded " + object_id + " from " + marker.mesh_resource +
" as marker.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
except AssimpError as ae:
loginfo("Problem loading " + object_id + " collision object: " +
str(ae))
def add_cylinder(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
# Cylinders are special - they are not supported directly by
# moveit_commander. It must be published manually to collision scene
cyl = CollisionObject()
cyl.operation = CollisionObject.ADD
cyl.id = object_id
cyl.header = stamped_pose.header
prim = SolidPrimitive()
prim.type = SolidPrimitive.CYLINDER
prim.dimensions = [size[0], size[1]]
cyl.primitives = [prim]
cyl.primitive_poses = [stamped_pose.pose]
# self.scene_pub.publish(cyl)
marker = self.make_marker_stub(stamped_pose,
[size[1] * 2, size[2] * 2, size[0]],
color=color)
marker.type = Marker.CYLINDER
self.publish_marker(object_id, marker)
sleep(0.5)
logdebug("Loaded " + object_id +
" as cylindrical collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def add_sphere(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
self.psi.add_sphere(object_id, stamped_pose, size[0])
loginfo("got past adding collision scene object")
marker = self.make_marker_stub(
stamped_pose, [size[0] * 2, size[1] * 2, size[2] * 2],
color=color)
marker.type = Marker.SPHERE
self.publish_marker(object_id, marker)
sleep(0.5)
loginfo("Loaded " + object_id + " as collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def add_box(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
self.psi.add_box(object_id, stamped_pose, size)
marker = self.make_marker_stub(stamped_pose, size, color=color)
marker.type = Marker.CUBE
self.publish_marker(object_id, marker)
sleep(0.5)
loginfo("Loaded " + object_id + " as collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def attach_box(self,
link,
object_id,
frame,
size,
attach_to_link,
pose=None,
position=None,
orientation=None,
rpy=None):
"""TODO: color is not yet supported, since it's not internally
supported by psi.attach_box. Basically duplicate this method
but with color support."""
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy),
self.psi.attach_box(link, object_id, stamped_pose, size,
attach_to_link)
sleep(0.5)
loginfo("Attached " + object_id + " as collision object.")
except ROSException as e:
loginfo("Problem attaching " + object_id + " collision object: " +
str(e))
@staticmethod
def make_stamped_pose(frame,
pose=None,
position=None,
orientation=None,
rpy=None):
if orientation is not None and rpy is not None:
logwarn("Collision object has both orientation (quaternion) and "
"Rotation (rpy) defined! Defaulting to quaternion "
"representation")
stamped_pose = PoseStamped()
stamped_pose.header.frame_id = frame
stamped_pose.header.stamp = Time.now()
# use a pose if one is provided, otherwise, make your own from the
# position and orientation.
if pose is not None:
stamped_pose.pose = pose
else:
stamped_pose.pose.position.x = position[0]
stamped_pose.pose.position.y = position[1]
stamped_pose.pose.position.z = position[2]
# for orientation, allow either quaternion or RPY specification
if orientation is not None:
stamped_pose.pose.orientation.x = orientation[0]
stamped_pose.pose.orientation.y = orientation[1]
stamped_pose.pose.orientation.z = orientation[2]
stamped_pose.pose.orientation.w = orientation[3]
elif rpy is not None:
quaternion = transformations.quaternion_from_euler(
rpy[0], rpy[1], rpy[2])
stamped_pose.pose.orientation.x = quaternion[0]
stamped_pose.pose.orientation.y = quaternion[1]
stamped_pose.pose.orientation.z = quaternion[2]
stamped_pose.pose.orientation.w = quaternion[3]
else:
stamped_pose.pose.orientation.w = 1 # make basic quaternion
return stamped_pose
# Fill a ROS Marker message with the provided data
def make_marker_stub(self, stamped_pose, size=None, color=None):
if color is None:
color = (0.5, 0.5, 0.5, 1.0)
if size is None:
size = (1, 1, 1)
marker = Marker()
marker.header = stamped_pose.header
# marker.ns = "collision_scene"
marker.id = self.next_id
marker.lifetime = Time(0) # forever
marker.action = Marker.ADD
marker.pose = stamped_pose.pose
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
if len(color) == 4:
alpha = color[3]
else:
alpha = 1.0
marker.color.a = alpha
marker.scale.x = size[0]
marker.scale.y = size[1]
marker.scale.z = size[2]
self.next_id += 1
return marker
def make_new_marker_array_msg(self):
ma = MarkerArray()
return ma
# publish a single marker
def publish_marker(self, object_id, marker):
loginfo("Publishing marker for object {}".format(object_id))
self.marker_array = self.make_new_marker_array_msg()
self.marker_array.markers.append(marker)
self.vis_pub.publish(self.marker_array)
self.objects[object_id] = marker
# Remove the provided object from the world.
def remove(self, object_id):
# if object_id not in self.objects:
# logwarn("PlanningSceneHelper was not used to create object with id "
# + object_id + ". Attempting to remove it anyway.")
try:
# first remove the actual collision object
self.psi.remove_world_object(object_id)
if object_id in self.objects:
# then remove the marker associated with it
marker = self.objects[object_id]
marker.action = Marker.DELETE
self.publish_marker(object_id, marker)
self.objects.pop(object_id)
logdebug("Marker for collision object " + object_id + " removed.")
except ROSException as e:
loginfo("Problem removing " + object_id +
" from collision scene:" + str(e))
return
except KeyError as e:
loginfo("Problem removing " + object_id +
" from collision scene:" + str(e))
return
loginfo("Model " + object_id + " removed from collision scene.")
# Remove the provided attached collision object.
def remove_attached(self, link, object_id):
try:
self.psi.remove_attached_object(link=link, name=object_id)
loginfo("Attached object '" + object_id +
"' removed from collision scene.")
except:
loginfo("Problem attached object '" + object_id +
"' removing from collision scene.")
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)
