def __init__(self): self._scene = PlanningSceneInterface() # clear the scene self._scene.remove_world_object() self.robot = RobotCommander() # pause to wait for rviz to load rospy.sleep(4) floor_pose = [0, 0, -0.12, 0, 0, 0, 1] floor_dimensions = [4, 4, 0.02] box1_pose = [0.70, 0.70, 0.25, 0, 0, 0, 1] box1_dimensions = [0.5, 0.5, 0.5] box2_pose = [0.5, -0.5, 0.60, 0, 0, 0, 1] box2_dimensions = [0.25, 0.25, 0.25] box3_pose = [0.5, -0.5, 0.24, 0, 0, 0, 1] box3_dimensions = [0.48, 0.48, 0.48] box4_pose = [-0.8, 0.7, 0.5, 0, 0, 0, 1] box4_dimensions = [0.4, 0.4, 0.4] self.add_box_object("floor", floor_dimensions, floor_pose) self.add_box_object("box1", box1_dimensions, box1_pose) self.add_box_object("box2", box2_dimensions, box2_pose) self.add_box_object("box3", box3_dimensions, box3_pose) self.add_box_object("box4", box4_dimensions, box4_pose)
def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param( '~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.1) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) rospy.sleep(1.0) # self.setup() # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) rospy.loginfo("added table") rospy.sleep(2.0) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) rospy.loginfo("added grasping box") rospy.sleep(1.0) # Define target place pose: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x self._pose_place.position.y = self._pose_coke_can.position.y - 0.06 self._pose_place.position.z = self._pose_coke_can.position.z self._pose_place.orientation = Quaternion( *quaternion_from_euler(0.0, 0.0, 0.0))
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('start_pos') # 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, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 10 seconds per planning attempt right_arm.set_planning_time(10) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def initialize_environment(): # Initialize environment rospy.init_node('Baxter_Env') robot = RobotCommander() scene = PlanningSceneInterface() scene._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=0) rospy.sleep(2) return scene, robot
def test_namespace(self): self.scene = PlanningSceneInterface() expected_resolved_co_name = "/test_ros_namespace/collision_object" expected_resolved_aco_name = "/test_ros_namespace/attached_collision_object" self.assertEqual(self.scene._pub_co.resolved_name, expected_resolved_co_name) self.assertEqual(self.scene._pub_aco.resolved_name, expected_resolved_aco_name)
def initialize_environment(): # Assume rosnode is already is initialized; scene = PlanningSceneInterface() scene._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=0) rospy.sleep(2) return scene
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_ik_demo') # 初始化需要使用move group控制的机械臂中的arm group rospy.Subscriber("obj_pose", Pose, obj_pose_get) # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' box2_id = 'box2' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) rospy.sleep(1) # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.3, 0.01] box1_size = [0.03, 0.001, 0.08] box2_size = [0.03, 0.03, 0.05] table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = table_size[0]/2.0+obj_pose.position.x - 0.05 table_pose.pose.position.y = obj_pose.position.y table_pose.pose.position.z = obj_pose.position.z - table_size[2] / 2.0 - box1_size[2]/2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = obj_pose.position.x box1_pose.pose.position.y = obj_pose.position.y box1_pose.pose.position.z = obj_pose.position.z box1_pose.pose.orientation.w = 1.0 # scene.add_box(box1_id, box1_pose, box1_size) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0, 0, 0, 1) self.setColor(box1_id, 0.8, 0, 0, 1.0) self.setColor(box2_id, 0.8, 0, 0, 1.0) # self.setColor(table_id, 0.8, 0, 0, 1.0) red # self.setColor(box1_id, 0.8, 0.4, 0, 1.0) yellow # self.setColor(box1_id, 1, 1, 1, 1.0) white # self.setColor(sphere_id, 0, 0, 0, 1) black # 将场景中的颜色设置发布 self.sendColors()
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move',anonymous=True) self.listener = tf.TransformListener() self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_roll = 0 self.goal_pitch = 0 self.goal_yaw = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.wpose = [] self.marker = [] self.tf_list = [] self.m_idd = [] self.m_pose_x = [] self.m_pose_y = [] self.m_pose_z = [] self.m_ori_w = [] self.m_ori_x = [] self.m_ori_y = [] self.m_ori_z = [] self.trans = [] self.rot = [] self.pose_goal = Pose() #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap()
def __init__(self): self._gdict = {} self._group_name = "" self._prev_group_name = "" self._planning_scene_interface = PlanningSceneInterface() self._last_plan = None self._db_host = None self._db_port = 33829 self._trace = False
def __init__(self): self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.p = PoseStamped() self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) rospy.loginfo("Connecting to clear octomap service...") self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo("Connected!")
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_fk_demo', anonymous=True) # 初始化move_group控制的机械臂中的arm group self.arm = moveit_commander.MoveGroupCommander('arm') self.gripper = moveit_commander.MoveGroupCommander('gripper') # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # 创建一个发布抓取姿态的发布者 self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) scene = PlanningSceneInterface() # 机械臂的允许误差值 self.arm.set_goal_joint_tolerance(0.001) self.gripper.set_goal_joint_tolerance(0.001) self.arm_goal = [0, 0, 0, 0, 0, 0] self.colors = dict() target_size = [0.02, 0.01, 0.12] target_id = 'test' # 设置目标物体的位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = "base_link" target_pose.pose.position.x = 0.82 target_pose.pose.position.y = 0.32 target_pose.pose.position.z = 0 target_pose.pose.orientation.w = 1.0 # 将抓取的目标物体加入场景中 scene.add_box(target_id, target_pose, target_size) # 将目标物体设置为黄色 self.setColor(target_id, 0.9, 0.9, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 控制机械臂竖起 # self.arm.set_named_target('up') # self.arm.go() # rospy.sleep(2) # 控制夹爪运动 # self.gripper.set_joint_value_target(0.6) # self.gripper.go() # rospy.sleep(2) #self.reset() self.arm.set_named_target('up') self.arm.go() rospy.sleep(2)
def init_scene(self): world_objects = [ "table", "tui", "monitor", "overHead", "wall", "farWall", "frontWall", "backWall", "blockProtector", "rearCamera" ] self.robot = RobotCommander() self.scene = PlanningSceneInterface() for obj in world_objects: self.scene.remove_world_object(obj) rospy.sleep(0.5) self.tuiPose = PoseStamped() self.tuiPose.header.frame_id = self.robot.get_planning_frame() self.tuiPose.pose.position = Point(0.0056, -0.343, -0.51) self.tuiDimension = (0.9906, 0.8382, 0.8836) self.overHeadPose = PoseStamped() self.overHeadPose.header.frame_id = self.robot.get_planning_frame() self.overHeadPose.pose.position = Point(0.0056, 0.0, 0.97) self.overHeadDimension = (0.9906, 0.8382, 0.05) self.blockProtectorPose = PoseStamped() self.blockProtectorPose.header.frame_id = self.robot.get_planning_frame( ) self.blockProtectorPose.pose.position = Point( 0.0056, -0.343, -0.51 + self.cruising_height) self.blockProtectorDimension = (0.9906, 0.8382, 0.8636) self.wallPose = PoseStamped() self.wallPose.header.frame_id = self.robot.get_planning_frame() self.wallPose.pose.position = Point(-0.858, -0.343, -0.3048) self.wallDimension = (0.6096, 2, 1.35) self.farWallPose = PoseStamped() self.farWallPose.header.frame_id = self.robot.get_planning_frame() self.farWallPose.pose.position = Point(0.9, -0.343, -0.3048) self.farWallDimension = (0.6096, 2, 3.35) self.frontWallPose = PoseStamped() self.frontWallPose.header.frame_id = self.robot.get_planning_frame() self.frontWallPose.pose.position = Point(0.0056, -0.85, -0.51) self.frontWallDimension = (1, 0.15, 4) self.backWallPose = PoseStamped() self.backWallPose.header.frame_id = self.robot.get_planning_frame() self.backWallPose.pose.position = Point(0.0056, 0.55, -0.51) self.backWallDimension = (1, 0.005, 4) self.rearCameraPose = PoseStamped() self.rearCameraPose.header.frame_id = self.robot.get_planning_frame() self.rearCameraPose.pose.position = Point(0.65, 0.45, -0.51) self.rearCameraDimension = (0.5, 0.5, 2) rospy.sleep(0.5) self.scene.add_box("tui", self.tuiPose, self.tuiDimension) self.scene.add_box("wall", self.wallPose, self.wallDimension) self.scene.add_box("farWall", self.farWallPose, self.farWallDimension) self.scene.add_box("overHead", self.overHeadPose, self.overHeadDimension) self.scene.add_box("backWall", self.backWallPose, self.backWallDimension) self.scene.add_box("frontWall", self.frontWallPose, self.frontWallDimension) self.scene.add_box("rearCamera", self.rearCameraPose, self.rearCameraDimension)
def get_planning_scene_interface(): ''' Gets the planning_scene_interface for this process. ''' global _psi with _psi_creation_lock: if _psi == None: _psi = PlanningSceneInterface() return _psi
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.detected_names = rospy.get_param( '/darknet_ros/yolo_model/detection_classes/names') self.object_pose_sub = rospy.Subscriber( '/cluster_decomposer/centroid_pose_array', PoseArray, self.collectJsk) self.listener = tf.TransformListener() display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.ii = 0 global goal_x global goal_y global goal_z global ori_w global ori_x global ori_y global ori_z self.distance = 0 self.tomato = [] self.position_list = [] self.orientation_list = [] self.tomato_pose = Pose() self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.calculated_tomato_coor = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(10) self.robot_arm.set_num_planning_attempts(10) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True)
def handle_clean_moveit_scene(self, req): scene = PlanningSceneInterface() rospy.sleep(1) # clean the scene #scene.remove_world_object('table_box') scene.remove_world_object('obj_mesh') response = ManageMoveitSceneResponse() response.success = True return response
def __init__(self): self.scene = PlanningSceneInterface() # self.pickplace = PickPlaceInterface("right_arm", "right_gripper", verbose=True, plan_only = False) # self.move_group = MoveGroupInterface("right_arm", "base_footprint", plan_only = False) self.left_arm = MoveGroupCommander("left_arm") # rospy.sleep(6.0) self.right_arm = MoveGroupCommander("right_arm") # rospy.sleep(6.0) self.dual_arm_group = MoveGroupCommander("dual_arm") # rospy.sleep(6.0) self.grippers = MoveGroupCommander("grippers")
def __init__(self): # create a RobotCommander self.robot = RobotCommander() # create a PlanningSceneInterface object self.scene = PlanningSceneInterface() # create arm self.arm = Arm("manipulator") # create gripper self.gripper = Gripper()
def __init__(self): rospy.loginfo("Initalizing PickAndPlaceServer...") self.sg = SphericalGrasps() rospy.loginfo("Connecting to pickup AS") self.pickup_ac = SimpleActionClient('/pickup', PickupAction) self.pickup_ac.wait_for_server() rospy.loginfo("Succesfully connected.") rospy.loginfo("Connecting to place AS") self.place_ac = SimpleActionClient('/place', PlaceAction) self.place_ac.wait_for_server() rospy.loginfo("Succesfully connected.") self.planning_scene_interface = PlanningSceneInterface() rospy.loginfo("Connecting to /get_planning_scene service") self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.scene_srv.wait_for_service() rospy.loginfo("Connected.") rospy.loginfo("Connecting to clear octomap service...") self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo("Connected!") # Get the object size self.object_height = rospy.get_param('~object_height') self.object_width = rospy.get_param('~object_width') self.object_depth = rospy.get_param('~object_depth') self.surface_height = rospy.get_param('~surface_height') self.surface_width = rospy.get_param('~surface_width') self.surface_depth = rospy.get_param('~surface_depth') # Get the links of the end effector exclude from collisions self.links_to_allow_contact = rospy.get_param( '~links_to_allow_contact', None) if self.links_to_allow_contact is None: rospy.logwarn( "Didn't find any links to allow contacts... at param ~links_to_allow_contact" ) else: rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact)) self.pick_as = SimpleActionServer('/pickup_pose', PickUpPoseAction, execute_cb=self.pick_cb, auto_start=False) self.pick_as.start() self.place_as = SimpleActionServer('/place_pose', PickUpPoseAction, execute_cb=self.place_cb, auto_start=False) self.place_as.start()
def __init__(self): self.node_name = "PickAndPlaceServer" rospy.loginfo("Initalizing PickAndPlaceServer...") self.sg = SphericalGrasps() # Get the object size self.object_height = 0.1 self.object_width = 0.05 self.object_depth = 0.05 self.pick_pose = rospy.get_param('~pickup_marker_pose') self.place_pose = rospy.get_param('~place_marker_pose') rospy.loginfo("%s: Waiting for pickup action server...", self.node_name) self.pickup_ac = SimpleActionClient('/pickup', PickupAction) connected = self.pickup_ac.wait_for_server(rospy.Duration(3000)) if not connected: rospy.logerr("%s: Could not connect to pickup action server", self.node_name) exit() rospy.loginfo("%s: Connected to pickup action server", self.node_name) rospy.loginfo("%s: Waiting for place action server...", self.node_name) self.place_ac = SimpleActionClient('/place', PlaceAction) if not self.place_ac.wait_for_server(rospy.Duration(3000)): rospy.logerr("%s: Could not connect to place action server", self.node_name) exit() rospy.loginfo("%s: Connected to place action server", self.node_name) rospy.loginfo("Connecting to /get_planning_scene service") self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.scene_srv.wait_for_service() self.scene = PlanningSceneInterface() rospy.loginfo("Planning scene created.") rospy.loginfo("Connecting to clear octomap service...") self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo("Connected!") # Get the links of the end effector exclude from collisions self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None) if self.links_to_allow_contact is None: rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact") else: rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact)) self.pick_as = SimpleActionServer(self.pick_pose, PickUpPoseAction, execute_cb=self.pick_cb, auto_start=False) self.pick_as.start() self.place_as = SimpleActionServer(self.place_pose, PickUpPoseAction, execute_cb=self.place_cb, auto_start=False) self.place_as.start()
def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, moveit_msgs.msg.PickupAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() rospy.loginfo('Action Service Loaded') self.robot = Phantomx_Pincher() rospy.loginfo('Moveit Robot Commander Loaded') self.scene = PlanningSceneInterface() rospy.loginfo('Moveit Planning Scene Loaded') rospy.loginfo('Pick action is ok. Awaiting for connections')
def __init__(self): self._scene = PlanningSceneInterface() # clear the scene self._scene.remove_world_object() self.robot = RobotCommander() # pause to wait for rviz to load print "============ Waiting while RVIZ displays the scene with obstacles..." # TODO: need to replace this sleep by explicitly waiting for the scene to be updated. rospy.sleep(2)
def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object2_name = rospy.get_param('~grasp_object_name', 'Grasp_Object-2') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param( '~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.1) # Create (debugging) publishers:创建(调试)发布者: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander:创建规划场景和机器人命令: self._scene = PlanningSceneInterface() #未知 self._robot = RobotCommander() #未知 rospy.sleep(0.5) # Clean the scene:清理现场: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._grasp_object2_name) self._scene.remove_world_object(self._table_object_name) # Add table and Coke can objects to the planning scene:将桌子和可乐罐对象添加到计划场景: self._pose_table = self._add_table(self._table_object_name) #增加桌子 self._pose_coke_can = self._add_grasp_block_( self._grasp_object_name) #增加障碍物块 self._pose_coke_can = self._add_grasp_fanfkuai_( self._grasp_object2_name) #增加障碍物块 rospy.sleep(0.5)
def __init__(self): rospy.wait_for_service("gazebo/spawn_sdf_model") self.spawn_model_srv = rospy.ServiceProxy("gazebo/spawn_sdf_model", SpawnModel) rospy.wait_for_service("gazebo/spawn_sdf_model") self.delete_model_srv = rospy.ServiceProxy("gazebo/delete_model", DeleteModel) self.scene = PlanningSceneInterface() self.robot = RobotCommander() rospy.sleep(1) self.object_list = {}
def __init__(self, name): """ Initialize MoveGroupCommander object @param name - name of the MoveIt group """ self._name = name self._move_group_commander = MoveGroupCommander(name) self._robot_commander = RobotCommander() self._robot_name = self._robot_commander._r.get_robot_name() self.refresh_named_targets() self._warehouse_name_get_srv = rospy.ServiceProxy( "get_robot_state", GetState) self._planning_scene = PlanningSceneInterface() self._joint_states_lock = threading.Lock() self._joint_states_listener = \ rospy.Subscriber("joint_states", JointState, self._joint_states_callback, queue_size=1) self._joints_position = {} self._joints_velocity = {} self._joints_effort = {} self._joints_state = None self._clients = {} self.__plan = None self._controllers = {} rospy.wait_for_service('compute_ik') self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK) controller_list_param = rospy.get_param("/move_group/controller_list") # create dictionary with name of controllers and corresponding joints self._controllers = { item["name"]: item["joints"] for item in controller_list_param } self._set_up_action_client(self._controllers) self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) threading.Thread(None, rospy.spin)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_ik_demo') # 初始化需要使用move group控制的机械臂中的arm group rospy.Subscriber("obj_pose", Pose, obj_pose_get) # 设置场景物体的颜色 # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) rospy.sleep(1) reference_frame = 'base_link' # 设置桌面的高度 table_ground = 0.37 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.3, 0.01] box1_size = [0.01, 0.01, 0.19] table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = -table_size[0] / 2.0 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = -0.09 box1_pose.pose.position.y = table_size[0] / 2.0 box1_pose.pose.position.z = 0.18 + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size)
def set_scene(): self.scene_pub = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10) self.scene = PlanningSceneInterface() rospy.sleep(2) self.add_collision_object('turtlebot', 0.8, 0., -0.65, .5, 1.5, .33) self.add_collision_object('right_wall', 0., 0.65, 0., 4., .1, 4.) self.add_collision_object('left_wall', 0., -0.8, 0., 4., .1, 4.) self.add_collision_object('back_wall', -0.6, 0., 0., .1, 4., 4.) self.add_collision_object('destination_table', .5, -.4, -.35, 1, .5, .5) self.plan_scene = PlanningScene() self.plan_scene.is_diff = True self.scene_pub.publish(self.plan_scene)
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.Pose_goal = Pose() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) # robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) print("====== move plan go to home 1 ======") self.robot_arm.set_named_target("up") # go to goal state. self.robot_arm.go() rospy.sleep(2)
def __init__(self, name): # stuff for grasp planning rospy.loginfo("Getting a TransformListener...") self.tf_listener = tf.TransformListener() rospy.loginfo("Getting a TransformBroadcaster...") self.tf_broadcaster = tf.TransformBroadcaster() rospy.loginfo("Initializing a ClusterBoundingBoxFinder...") self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link") self.last_clusters = None rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...") self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback) if DEBUG_MODE: self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped) rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...") self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction) self.pickup_ac.wait_for_server() rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...") self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction) self.place_ac.wait_for_server() rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...") self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction) self.grasps_ac.wait_for_server() rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...") self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty) self.depth_service.wait_for_service() rospy.loginfo("Getting a PlanningSceneInterface instance...") self.scene = PlanningSceneInterface() # blocking action server rospy.loginfo("Creating Action Server '" + name + "'...") self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False) self.as_feedback = ObjectManipulationFeedback() self.as_result = ObjectManipulationActionResult() self.current_goal = None # Take care of left and right arm grasped stuff self.right_hand_object = None self.left_hand_object = None self.current_side = 'right' rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!") self.grasp_obj_as.start()
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('PP_server_node') self._scene = PlanningSceneInterface('world') self._scene_pub = rospy.Publisher('planning_scene', PlanningScene) self.colors = dict() self.max_pick_attempts = 5 self.max_place_attempts = 5 # initialize action server, type is PoseAndSizeAction defined in kinova_msgs/action, # callback function is PickAndPlace self._server = actionlib.SimpleActionServer( "PickAndPlace", kinova_msgs.msg.PoseAndSizeAction, execute_cb=self.PickAndPlace, auto_start=False) self._server.start() # start action server
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True)