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 = 'ｂase_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)
