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)