def testsimple(self): client = SimpleActionClient('reference_action', TestAction) self.assert_(client.wait_for_server(rospy.Duration(10.0)), 'Could not connect to the action server') goal = TestGoal(1) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) self.assertEqual("The ref server has succeeded", client.get_goal_status_text()) goal = TestGoal(2) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.ABORTED, client.get_state()) self.assertEqual("The ref server has aborted", client.get_goal_status_text())
def execute(self, userdata): # TODO: Make it look less stupid when stuck while True: # absolutely need to get the transforms. try: # TODO: make sure we can get a tf even with time lag t, r = tf_listener.lookupTransform('map', 'base_link', rospy.Time.now()) break except: pass p = self.initial_pose.position o = self.initial_pose.orientation p.x, p.y, p.z = t o.x, o.y, o.z, o.w = r client = SimpleActionClient('move_base', MoveBaseAction) rospy.loginfo('Waiting for MOVE_BASE SERVER ...') client.wait_for_server() rospy.loginfo('MOVE_BASE SERVER IS UP!') for i in range(self.n_attempts): if rospy.is_shutdown(): exit() rospy.loginfo("Attempting Unstuck, {} / {}".format( i + 1, self.n_attempts)) goal = self.make_goal() client.send_goal(goal) start = rospy.Time.now() while (rospy.Time.now() - start).to_sec() < 30.0: # wait 30 sec. for success if rospy.is_shutdown(): exit() if client.wait_for_result( rospy.Duration(1.0) ): # if move_base decides earlier that a goal is impossible... res = client.get_state() text = client.get_goal_status_text() print text if res == 3: ## SUCCEEDED return 'succeeded' return 'aborted'
class Pick_Place: 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() rospy.sleep(1.0) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # # self._pose_table = self._scene.get_object_poses(self._table_object_name) # self._pose_coke_can = self._scene.get_object_poses(self._grasp_object_name) # self.setup() # Clean the scene: self._scene.remove_world_object(self._table_object_name) rospy.sleep(1.0) self._scene.remove_world_object(self._grasp_object_name) rospy.sleep(1.0) # # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) rospy.sleep(2.0) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) rospy.sleep(2.0) # 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)) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Create move group 'place' action client: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') # Place Coke can object on another place on the support surface (table): # while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): # rospy.logwarn('Place failed! Retrying ...') # rospy.sleep(1.0) # # rospy.loginfo('Place successfully') def 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) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) self._scene.remove_attached_object() def _add_table(self, name): rospy.loginfo("entered table") p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = -0.2 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (9 , 9, 0.02)) return p.pose def _add_grasp_block_(self, name): rospy.loginfo("entered box grabber") p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.05 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.1, 0.1, 0.1)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle ) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.2 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.2 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_coke_can, width) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg) def _add_objects(self): """ Here add all the objects that you want to add to the _scene """ self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
class PathPlannerNode(object): """ This is a ROS node that is responsible for planning and executing the a path through the field. """ def __init__(self): # Setup ROS node rospy.init_node('path_planner') # ROS params self.cut_spacing = rospy.get_param("~coverage_spacing", 0.4) # Setup publishers and subscribers rospy.Subscriber('heatmap_area', PolygonStamped, self.field_callback) self.path_marker_pub = rospy.Publisher('visualization_marker', MarkerArray, latch=True) rospy.Subscriber('/odom', Odometry, self.odom_callback) # Setup initial variables self.field_shape = None self.field_frame_id = None self.path = None self.path_status = None self.path_markers = None self.start_path_following = False self.robot_pose = None self.goal_state = None self.current_destination = None self.testing = False self.current_distance = None self.previous_destination = None self.clear_costmaps = rospy.ServiceProxy('move_base/clear_costmaps', Empty) self.just_reset = False self.timeout = False self.heading = 0 # Spin until shutdown or we are ready for path following rate = rospy.Rate(10.0) while not rospy.is_shutdown(): rate.sleep() if self.start_path_following: # Run until stopped # Setup path following self.setup_path_following(self.heading) # Iterate on path following while not rospy.is_shutdown(): if not self.step_path_following(): break self.start_path_following = False def field_callback(self, msg): """ Handles new field polygons, has to be called at least once before planning happens. """ # Convert the PolygonStamped into a shapely polygon temp_points = [] for point in msg.polygon.points: temp_points.append( (float(point.x), float(point.y)) ) self.field_shape = geo.Polygon(temp_points) self.field_frame_id = msg.header.frame_id self.start_path_following = True def odom_callback(self, msg): """ Watches for the robot's Odometry data, which is used in the path planning as the initial robot position. """ self.robot_pose = msg def plan_path(self, field_polygon, origin=None, degrees=0): """ This is called after a field polygon has been received. This uses the automow_planning.coverage module to plan a coverage path using the field geometry. The path consists of a series of waypoints. """ # Get the rotation to align with the longest edge of the polygon from maptools import rotation_tf_from_longest_edge, RotationTransform rotation = rotation_tf_from_longest_edge(field_polygon) rotation = RotationTransform(rotation.angle + degrees) # Rotate the field polygon print "Rotate the field polygon" from maptools import rotate_polygon_from, rotate_polygon_to transformed_field_polygon = rotate_polygon_from(field_polygon, rotation) # Decompose the rotated field into a series of waypoints from coverage import decompose print origin if origin is not None: point_mat = np.mat([[origin[0], origin[1], 0]], dtype='float64').transpose() origin = rotation.irm * point_mat origin = (origin[0,0], origin[1,0]) print(self.cut_spacing) transformed_path = decompose(transformed_field_polygon, origin=(origin[0], origin[1]), width=self.cut_spacing) # Rotate the transformed path back into the source frame from maptools import rotate_from, rotate_to self.path = rotate_to(np.array(transformed_path), rotation) # Calculate headings and extend the waypoints with them self.path = self.calculate_headings(self.path, rotation) # Set the path_status to 'not_visited' self.path_status = [] for waypoint in self.path: self.path_status.append('not_visited') # Visualize the data self.visualize_path(self.path, self.path_status) def calculate_headings(self, path, rotation): """ Calculates the headings between paths and adds them to the waypoints. """ new_path = [] for index, waypoint in enumerate(path): new_path.append(list(path[index])) # If the end, copy the previous heading if index == 0: new_path[index].append(0) continue # Calculate the angle between this waypoint and the next dx = path[index][0] - path[index-1][0] dy = path[index][1] - path[index-1][1] from math import atan2, pi #if you want to use turning path, use atan #heading = atan2(dy, dx) heading = rotation.w + 1.5708 new_path[index].append(heading) return new_path def visualize_path(self, path, path_status=None): """ Convenience function, calls both visualize_path{as_path, as_marker} """ # TODO: finish this (path as path viz) # self.visualize_path_as_path(path, path_status) self.visualize_path_as_marker(path, path_status) def visualize_path_as_path(self, path, path_status=None): """ Publishes a nav_msgs/Path msg containing the planned path. If path_status is not None then the only waypoints put in the nav_msgs/Path msg will be ones that are 'not_visited' or 'visiting'. """ now = rospy.Time.now() msg = Path() msg.header.stamp = now msg.header.frame_id = self.field_frame_id for index, waypoint in enumerate(path): # Only put 'not_visited', 'visiting', and the most recent 'visited' # in the path msg if path_status != None: # If not set, ignore if path_status[index] == 'visited': # if this one is visited try: # if the next one is visited too if path_status[index+1] == 'visited': # Then continue, because this one doesn't belong # in the path msg continue except KeyError as e: # incase index+1 is too big pass # Otherwise if belongs, put it in pose_msg = PoseStamped() pose_msg.header.stamp = now pose_msg.header.frame_id = self.field_frame_id pose_msg.pose.position.x = waypoint[0] pose_msg.pose.position.y = waypoint[1] msg.poses.append(pose_msg) def visualize_path_as_marker(self, path, path_status): """ Publishes visualization Markers to represent the planned path. Publishes the path as a series of spheres connected by lines. The color of the spheres is set by the path_status parameter, which is a list of strings of which the possible values are in ['not_visited', 'visiting', 'visited']. """ # Get the time now = rospy.Time.now() # If self.path_markers is None, initialize it if self.path_markers == None: self.path_markers = MarkerArray() # # If there are existing markers, delete them # markers_to_delete = MarkerArray() # if len(self.path_markers.markers) > 0: # for marker in self.path_markers.markers: # marker.action = Marker.DELETE # markers_to_delete.markers.append(marker) # self.path_marker_pub.publish(markers_to_delete) # Clear the path_markers self.path_markers = MarkerArray() line_strip_points = [] # Create the waypoint markers for index, waypoint in enumerate(path): waypoint_marker = Marker() waypoint_marker.header.stamp = now waypoint_marker.header.frame_id = self.field_frame_id waypoint_marker.ns = "waypoints" waypoint_marker.id = index waypoint_marker.type = Marker.ARROW if index == 0: waypoint_marker.type = Marker.CUBE waypoint_marker.action = Marker.MODIFY waypoint_marker.scale.x = 1 waypoint_marker.scale.y = 1 waypoint_marker.scale.z = 0.25 point = Point(waypoint[0], waypoint[1], 0) waypoint_marker.pose.position = point # Store the point for the line_strip marker line_strip_points.append(point) # Set the heading of the ARROW quat = qfe(0, 0, waypoint[2]) waypoint_marker.pose.orientation.x = quat[0] waypoint_marker.pose.orientation.y = quat[1] waypoint_marker.pose.orientation.z = quat[2] waypoint_marker.pose.orientation.w = quat[3] # Color is based on path_status status = path_status[index] if status == 'not_visited': waypoint_marker.color = ColorRGBA(1,0,0,0.5) elif status == 'visiting': waypoint_marker.color = ColorRGBA(0,1,0,0.5) elif status == 'visited': waypoint_marker.color = ColorRGBA(0,0,1,0.5) else: rospy.err("Invalid path status.") waypoint_marker.color = ColorRGBA(1,1,1,0.5) # Put this waypoint Marker in the MarkerArray self.path_markers.markers.append(waypoint_marker) # Create the line_strip Marker which connects the waypoints line_strip = Marker() line_strip.header.stamp = now line_strip.header.frame_id = self.field_frame_id line_strip.ns = "lines" line_strip.id = 0 line_strip.type = Marker.LINE_STRIP line_strip.action = Marker.ADD line_strip.scale.x = 0.1 line_strip.color = ColorRGBA(0,0,1,0.5) line_strip.points = line_strip_points self.path_markers.markers.append(line_strip) # Publish the marker array self.path_marker_pub.publish(self.path_markers) def setup_path_following(self, degrees=0): """ Sets up the node for following the planned path. Will wait until the initial robot pose is set and until the move_base actionlib service is available. """ # Create the actionlib service self.move_base_client = SimpleActionClient('move_base', MoveBaseAction) connected_to_move_base = False dur = rospy.Duration(1.0) # If testing prime the robot_pose if self.testing: self.robot_pose = Odometry() self.robot_pose.pose.pose.position.x = 0 self.robot_pose.pose.pose.position.y = 0 # Wait for the field shape while self.field_shape == None: # Check to make sure ROS is ok still if rospy.is_shutdown(): return # Print message about the waiting msg = "Qualification: waiting on the field shape." rospy.loginfo(msg) rospy.Rate(1.0).sleep() # Wait for the robot position while self.robot_pose == None: # Check to make sure ROS is ok still if rospy.is_shutdown(): return # Print message about the waiting msg = "Qualification: waiting on initial robot pose." rospy.loginfo(msg) rospy.Rate(1.0).sleep() # Now we should plan a path using the robot's initial pose origin = (self.robot_pose.pose.pose.position.x, self.robot_pose.pose.pose.position.y) degrees = 90 self.plan_path(self.field_shape, origin, degrees) rospy.loginfo("Path Planner: path planning complete.") # Now wait for move_base while not connected_to_move_base: # Wait for the server for a while connected_to_move_base = self.move_base_client.wait_for_server(dur) # Check to make sure ROS is ok still if rospy.is_shutdown(): return # Update the user on the status of this process msg = "Path Planner: waiting on move_base." rospy.loginfo(msg) # Now we are ready to start feeding move_base waypoints return def get_next_waypoint_index(self): """ Figures out what the index of the next waypoint is. Iterates through the path_status's and finds the visiting one, or the next not_visited one if not visiting on exists. """ for index, status in enumerate(self.path_status): if status == 'visited': continue if status == 'visiting': return index if status == 'not_visited': return index # If I get here then there are no not_visited and we are done. return None def distance(self, p1, p2): """Returns the distance between two points.""" from math import sqrt dx = p2.target_pose.pose.position.x - p1.target_pose.pose.position.x dy = p2.target_pose.pose.position.y - p1.target_pose.pose.position.y return sqrt(dx**2 + dy**2) def step_path_following(self): """ Steps the path following system, checking if new waypoints should be sent, if a timeout has occurred, or if path following needs to be paused. """ # Visualize the data self.visualize_path(self.path, self.path_status) # Get the next (or current) waypoint current_waypoint_index = self.get_next_waypoint_index() # If the index is None, then we are done path planning if current_waypoint_index == None: rospy.loginfo("Path Planner: Done.") return False if current_waypoint_index == 0: self.path_status[current_waypoint_index] = 'visited' # Get the waypoint and status current_waypoint = self.path[current_waypoint_index] current_waypoint_status = self.path_status[current_waypoint_index] # If the status is visited print current_waypoint if current_waypoint_status == 'visited': # This shouldn't happen... return True # If the status is not_visited then we need to push the goal if current_waypoint_status == 'not_visited': # Cancel any current goals #self.move_base_client.cancel_all_goals() # Set it to visiting self.path_status[current_waypoint_index] = 'visiting' # Push the goal to the actionlib server destination = MoveBaseGoal() destination.target_pose.header.frame_id = self.field_frame_id destination.target_pose.header.stamp = rospy.Time.now() # Set the target location destination.target_pose.pose.position.x = current_waypoint[0] destination.target_pose.pose.position.y = current_waypoint[1] # Calculate the distance if self.previous_destination == None: self.current_distance = 5.0 else: self.current_distance = self.distance(self.previous_destination, destination) # Set the heading quat = qfe(0, 0, current_waypoint[2]) destination.target_pose.pose.orientation.x = quat[0] destination.target_pose.pose.orientation.y = quat[1] destination.target_pose.pose.orientation.z = quat[2] destination.target_pose.pose.orientation.w = quat[3] # Send the desired destination to the actionlib server rospy.loginfo("Sending waypoint (%f, %f)@%f" % tuple(current_waypoint)) self.current_destination = destination self.move_base_client.send_goal(destination) if self.current_distance < self.cut_spacing + 0.1: rospy.sleep(0.1) self.move_base_client.cancel_goal() self.previous_destination = destination # If the status is visiting, then we just need to monitor the status temp_state = self.move_base_client.get_goal_status_text() if current_waypoint_status == 'visiting': if temp_state in ['ABORTED', 'SUCCEEDED']: self.path_status[current_waypoint_index] = 'visited' else: duration = rospy.Duration(2.0) from math import floor count = 0 self.move_base_client.wait_for_result() if self.timeout == True: if count == 6+int(self.current_distance*4): rospy.logwarn("Path Planner: move_base goal timeout occurred, clearing costmaps") # Cancel the goals self.move_base_client.cancel_all_goals() # Clear the cost maps self.clear_costmaps() # Wait 1 second rospy.Rate(1.0).sleep() # Then reset the current goal if not self.just_reset: self.just_reset = True self.path_status[current_waypoint_index] = 'not_visited' else: self.just_reset = False self.path_status[current_waypoint_index] = 'visited' return True self.path_status[current_waypoint_index] = 'visited' return True
class CokeCanPickAndPlace: 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('~arm', 'arm') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.6) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4) # 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(1.0) # 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) self._pose_coke_can = self._add_coke_can(self._grasp_object_name) 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.02 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)) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Create move group 'place' action client: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Place successfully') def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose def _add_coke_can(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.01, 0.01, 0.009)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = -1 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 1 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_coke_can, width) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg)
class PathExecutor: def __init__(self): self._result = ExecutePathResult() self._feedback = ExecutePathFeedback() self._action_name = "/path_executor/execute_path" self._as = SimpleActionServer( self._action_name, ExecutePathAction, execute_cb=self.execute_cb, auto_start=False ) self._as.start() rospy.loginfo("Server start") # initialise current path publisher self._path_publisher = rospy.Publisher("/path_executor/current_path", Path) # initialise Bug2 client if rospy.get_param("~use_obstacle_avoidance"): MOVE_SERVER = "/bug2/move_to" else: MOVE_SERVER = "/motion_controller/move_to" print MOVE_SERVER self._move_client = SimpleActionClient(MOVE_SERVER, MoveToAction) print "Connecting to bug server...", MOVE_SERVER self._move_client.wait_for_server() def move_to_next_node(self): print "moveto next node" if len(self._poses_chain): bug_goal = MoveToGoal() bug_goal.target_pose = self._poses_chain.pop(0) self._move_client.send_goal(bug_goal, done_cb=self.move_to_done_cb, feedback_cb=self.move_to_feedback_cb) else: return # self._result.visited = [True] # self._as.set_succeeded(self._result) def execute_cb(self, goal): # rospy.loginfo('CALLBACK') # self._feedback.reached = True # self._as.publish_feedback(self._feedback) rospy.loginfo("----- ----- -----") rospy.loginfo("Path acquired") r = rospy.Rate(10) self.MOVING = True self._poses_chain = goal.path.poses self.move_to_next_node() while self.MOVING: r.sleep() """ if self._as.is_preempt_requested(): rospy.loginfo() self._poses_chain = [] self._as.set_preempted() """ # self._path_publisher.publish(goal.path) """ Callbacks of MOVETO client """ def move_to_done_cb(self, state, result): print "MOVETO DONE CALLBACK" # for i in result: # print i # rospy.loginfo(type(result)) # print dir(state) # print type(state) print "STATE", state self.move_to_next_node() def move_to_feedback_cb(self, feedback): # print 'MOVETO FEEDBACK CALLBACK' print self._move_client.get_goal_status_text() # print dir(feedback) print type(feedback) # rospy.loginfo('FEEDBACK: '%feedback.status) pass
class PathPlannerNode(object): """ This is a ROS node that is responsible for planning and executing the a path through the field. """ def __init__(self): # Setup ROS node rospy.init_node('path_planner') # ROS params self.cut_spacing = rospy.get_param("~cut_spacing", 0.25) # Setup publishers and subscribers rospy.Subscriber('/field/cut_area', PolygonStamped, self.field_callback) self.path_marker_pub = rospy.Publisher('visualization_marker', MarkerArray, latch=True) rospy.Subscriber('/odom', Odometry, self.odom_callback) # Setup initial variables self.field_shape = None self.field_frame_id = None self.path = None self.path_status = None self.path_markers = None self.start_path_following = True self.robot_pose = None self.goal_state = None self.current_destination = None self.testing = False self.current_distance = None self.previous_destination = None self.clear_costmaps = rospy.ServiceProxy('/move_base/clear_costmaps', Empty) self.just_reset = False # Spin until shutdown or we are ready for path following rate = rospy.Rate(10.0) while not rospy.is_shutdown() and not self.start_path_following: rate.sleep() # If shutdown, return now if rospy.is_shutdown(): return # Run until stopped heading = 0 while not rospy.is_shutdown(): # Setup path following self.setup_path_following(heading) # Iterate on path following while not rospy.is_shutdown(): if not self.step_path_following(): break # Rotate 90 degrees, repeat if heading == 0: heading = 90 else: heading = 0 def field_callback(self, msg): """ Handles new field polygons, has to be called at least once before planning happens. """ # Convert the PolygonStamped into a shapely polygon temp_points = [] for point in msg.polygon.points: temp_points.append( (float(point.x), float(point.y)) ) self.field_shape = geo.Polygon(temp_points) self.field_frame_id = msg.header.frame_id def odom_callback(self, msg): """ Watches for the robot's Odometry data, which is used in the path planning as the initial robot position. """ self.robot_pose = msg def plan_path(self, field_polygon, origin=None, degrees=0): """ This is called after a field polygon has been received. This uses the automow_planning.coverage module to plan a coverage path using the field geometry. The path consists of a series of waypoints. """ # Get the rotation to align with the longest edge of the polygon from automow_planning.maptools import rotation_tf_from_longest_edge, RotationTransform rotation = rotation_tf_from_longest_edge(field_polygon) rotation = RotationTransform(rotation.w + degrees) # Rotate the field polygon from automow_planning.maptools import rotate_polygon_to transformed_field_polygon = rotate_polygon_to(field_polygon, rotation) # Decompose the rotated field into a series of waypoints from automow_planning.coverage import decompose print origin if origin is not None: point_mat = np.mat([[origin[0], origin[1], 0]], dtype='float64').transpose() origin = rotation.irm * point_mat origin = (origin[0,0], origin[1,0]) transformed_path = decompose(transformed_field_polygon, origin=(origin[0], origin[1]), width=self.cut_spacing) # Rotate the transformed path back into the source frame from automow_planning.maptools import rotate_from self.path = rotate_from(np.array(transformed_path), rotation) # Calculate headings and extend the waypoints with them self.path = self.calculate_headings(self.path) # Set the path_status to 'not_visited' self.path_status = [] for waypoint in self.path: self.path_status.append('not_visited') # Visualize the data self.visualize_path(self.path, self.path_status) def calculate_headings(self, path): """ Calculates the headings between paths and adds them to the waypoints. """ new_path = [] for index, waypoint in enumerate(path): new_path.append(list(path[index])) # If the end, copy the previous heading if index == 0: new_path[index].append(0) continue # Calculate the angle between this waypoint and the next dx = path[index][0] - path[index-1][0] dy = path[index][1] - path[index-1][1] from math import atan2, pi heading = atan2(dy, dx) new_path[index].append(heading) return new_path def visualize_path(self, path, path_status=None): """ Convenience function, calls both visualize_path{as_path, as_marker} """ # TODO: finish this (path as path viz) # self.visualize_path_as_path(path, path_status) self.visualize_path_as_marker(path, path_status) def visualize_path_as_path(self, path, path_status=None): """ Publishes a nav_msgs/Path msg containing the planned path. If path_status is not None then the only waypoints put in the nav_msgs/Path msg will be ones that are 'not_visited' or 'visiting'. """ now = rospy.Time.now() msg = Path() msg.header.stamp = now msg.header.frame_id = self.field_frame_id for index, waypoint in enumerate(path): # Only put 'not_visited', 'visiting', and the most recent 'visited' # in the path msg if path_status != None: # If not set, ignore if path_status[index] == 'visited': # if this one is visited try: # if the next one is visited too if path_status[index+1] == 'visited': # Then continue, because this one doesn't belong # in the path msg continue except KeyError as e: # incase index+1 is too big pass # Otherwise if belongs, put it in pose_msg = PoseStamped() pose_msg.header.stamp = now pose_msg.header.frame_id = self.field_frame_id pose_msg.pose.position.x = waypoint[0] pose_msg.pose.position.y = waypoint[1] msg.poses.append(pose_msg) def visualize_path_as_marker(self, path, path_status): """ Publishes visualization Markers to represent the planned path. Publishes the path as a series of spheres connected by lines. The color of the spheres is set by the path_status parameter, which is a list of strings of which the possible values are in ['not_visited', 'visiting', 'visited']. """ # Get the time now = rospy.Time.now() # If self.path_markers is None, initialize it if self.path_markers == None: self.path_markers = MarkerArray() # # If there are existing markers, delete them # markers_to_delete = MarkerArray() # if len(self.path_markers.markers) > 0: # for marker in self.path_markers.markers: # marker.action = Marker.DELETE # markers_to_delete.markers.append(marker) # self.path_marker_pub.publish(markers_to_delete) # Clear the path_markers self.path_markers = MarkerArray() line_strip_points = [] # Create the waypoint markers for index, waypoint in enumerate(path): waypoint_marker = Marker() waypoint_marker.header.stamp = now waypoint_marker.header.frame_id = self.field_frame_id waypoint_marker.ns = "waypoints" waypoint_marker.id = index waypoint_marker.type = Marker.ARROW if index == 0: waypoint_marker.type = Marker.CUBE waypoint_marker.action = Marker.MODIFY waypoint_marker.scale.x = 1 waypoint_marker.scale.y = 1 waypoint_marker.scale.z = 0.25 point = Point(waypoint[0], waypoint[1], 0) waypoint_marker.pose.position = point # Store the point for the line_strip marker line_strip_points.append(point) # Set the heading of the ARROW quat = qfe(0, 0, waypoint[2]) waypoint_marker.pose.orientation.x = quat[0] waypoint_marker.pose.orientation.y = quat[1] waypoint_marker.pose.orientation.z = quat[2] waypoint_marker.pose.orientation.w = quat[3] # Color is based on path_status status = path_status[index] if status == 'not_visited': waypoint_marker.color = ColorRGBA(1,0,0,0.5) elif status == 'visiting': waypoint_marker.color = ColorRGBA(0,1,0,0.5) elif status == 'visited': waypoint_marker.color = ColorRGBA(0,0,1,0.5) else: rospy.err("Invalid path status.") waypoint_marker.color = ColorRGBA(1,1,1,0.5) # Put this waypoint Marker in the MarkerArray self.path_markers.markers.append(waypoint_marker) # Create the line_strip Marker which connects the waypoints line_strip = Marker() line_strip.header.stamp = now line_strip.header.frame_id = self.field_frame_id line_strip.ns = "lines" line_strip.id = 0 line_strip.type = Marker.LINE_STRIP line_strip.action = Marker.ADD line_strip.scale.x = 0.1 line_strip.color = ColorRGBA(0,0,1,0.5) line_strip.points = line_strip_points self.path_markers.markers.append(line_strip) # Publish the marker array self.path_marker_pub.publish(self.path_markers) def setup_path_following(self, degrees=0): """ Sets up the node for following the planned path. Will wait until the initial robot pose is set and until the move_base actionlib service is available. """ # Create the actionlib service self.move_base_client = SimpleActionClient('move_base', MoveBaseAction) connected_to_move_base = False dur = rospy.Duration(1.0) # If testing prime the robot_pose if self.testing: self.robot_pose = Odometry() self.robot_pose.pose.pose.position.x = 0 self.robot_pose.pose.pose.position.y = 0 # Wait for the field shape while self.field_shape == None: # Check to make sure ROS is ok still if rospy.is_shutdown(): return # Print message about the waiting msg = "Qualification: waiting on the field shape." rospy.loginfo(msg) rospy.Rate(1.0).sleep() # Wait for the robot position while self.robot_pose == None: # Check to make sure ROS is ok still if rospy.is_shutdown(): return # Print message about the waiting msg = "Qualification: waiting on initial robot pose." rospy.loginfo(msg) rospy.Rate(1.0).sleep() # Now we should plan a path using the robot's initial pose origin = (self.robot_pose.pose.pose.position.x, self.robot_pose.pose.pose.position.y) self.plan_path(self.field_shape, origin, degrees) rospy.loginfo("Path Planner: path planning complete.") # Now wait for move_base while not connected_to_move_base: # Wait for the server for a while connected_to_move_base = self.move_base_client.wait_for_server(dur) # Check to make sure ROS is ok still if rospy.is_shutdown(): return # Update the user on the status of this process msg = "Path Planner: waiting on move_base." rospy.loginfo(msg) # Now we are ready to start feeding move_base waypoints return def get_next_waypoint_index(self): """ Figures out what the index of the next waypoint is. Iterates through the path_status's and finds the visiting one, or the next not_visited one if not visiting on exists. """ for index, status in enumerate(self.path_status): if status == 'visited': continue if status == 'visiting': return index if status == 'not_visited': return index # If I get here then there are no not_visited and we are done. return None def distance(self, p1, p2): """Returns the distance between two points.""" from math import sqrt dx = p2.target_pose.pose.position.x - p1.target_pose.pose.position.x dy = p2.target_pose.pose.position.y - p1.target_pose.pose.position.y return sqrt(dx**2 + dy**2) def step_path_following(self): """ Steps the path following system, checking if new waypoints should be sent, if a timeout has occurred, or if path following needs to be paused. """ # Visualize the data self.visualize_path(self.path, self.path_status) # Get the next (or current) waypoint current_waypoint_index = self.get_next_waypoint_index() # If the index is None, then we are done path planning if current_waypoint_index == None: rospy.loginfo("Path Planner: Done.") return False if current_waypoint_index == 0: self.path_status[current_waypoint_index] = 'visited' # Get the waypoint and status current_waypoint = self.path[current_waypoint_index] current_waypoint_status = self.path_status[current_waypoint_index] # If the status is visited if current_waypoint_status == 'visited': # This shouldn't happen... return True # If the status is not_visited then we need to push the goal if current_waypoint_status == 'not_visited': # Cancel any current goals self.move_base_client.cancel_all_goals() # Set it to visiting self.path_status[current_waypoint_index] = 'visiting' # Push the goal to the actionlib server destination = MoveBaseGoal() destination.target_pose.header.frame_id = self.field_frame_id destination.target_pose.header.stamp = rospy.Time.now() # Set the target location destination.target_pose.pose.position.x = current_waypoint[0] destination.target_pose.pose.position.y = current_waypoint[1] # Calculate the distance if self.previous_destination == None: self.current_distance = 5.0 else: self.current_distance = self.distance(self.previous_destination, destination) # Set the heading quat = qfe(0, 0, current_waypoint[2]) destination.target_pose.pose.orientation.x = quat[0] destination.target_pose.pose.orientation.y = quat[1] destination.target_pose.pose.orientation.z = quat[2] destination.target_pose.pose.orientation.w = quat[3] # Send the desired destination to the actionlib server rospy.loginfo("Sending waypoint (%f, %f)@%f" % tuple(current_waypoint)) self.current_destination = destination self.move_base_client.send_goal(destination) self.previous_destination = destination # If the status is visiting, then we just need to monitor the status temp_state = self.move_base_client.get_goal_status_text() if current_waypoint_status == 'visiting': if temp_state in ['ABORTED', 'SUCCEEDED']: self.path_status[current_waypoint_index] = 'visited' else: duration = rospy.Duration(1.0) from math import floor count = 0 while not self.move_base_client.wait_for_result(duration) and count != 6+int(self.current_distance*4): if rospy.is_shutdown(): return False count += 1 if count == 6+int(self.current_distance*4): rospy.logwarn("Path Planner: move_base goal timeout occurred, clearing costmaps") # Cancel the goals self.move_base_client.cancel_all_goals() # Clear the cost maps self.clear_costmaps() # Wait 1 second rospy.Rate(1.0).sleep() # Then reset the current goal if not self.just_reset: self.just_reset = True self.path_status[current_waypoint_index] = 'not_visited' else: self.just_reset = False self.path_status[current_waypoint_index] = 'visited' return True self.path_status[current_waypoint_index] = 'visited' return True
class Pick_Place: def __init__(self): # 检索参数: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._table2_object_name = rospy.get_param('~table_object_name', 'Grasp_Table2') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object2_name = rospy.get_param('~grasp_object_name', 'Grasp_Object2') 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) # 创建(调试)发布者: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # 创建规划现场,机器人指挥官: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # 清理现场: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._table2_object_name) self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._grasp_object2_name) # 将表和可乐罐对象添加到计划场景: self._pose_table = self._add_table(self._table_object_name) self._pose_table = self._add_table2(self._table2_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) self._pose_coke_can2 = self._add_grasp_block2_(self._grasp_object2_name) rospy.sleep(1.0) # 定义目标位置姿势: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x-0.5 self._pose_place.position.y = self._pose_coke_can.position.y-0.85 self._pose_place.position.z = self._pose_coke_can.position.z-0.3 self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above # 检索组 (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) self._arm.set_named_target('pickup') self._arm.go(wait=True) print("Pickup pose") # 创建抓取生成器“生成”动作客户端: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(1.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # 创建移动组“zhua取”操作客户端: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(1.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # 创建移动组“放置”动作客户端: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(1.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') print("pose_place: ", self._pose_place) # 放置可乐可能会在支撑表面(桌子)上的其他地方产生异物: while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Place successfully') print "-------------test1--------------" rospy.sleep(1.0) def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._table2_object_name) print "-------------test2------------------" def _add_table(self, name): """ 创建表并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.85 p.pose.position.y = 0.0 p.pose.position.z = 0.70 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (1, 1, 0.05)) print "-------------test3------------------" return p.pose def _add_table2(self, name): """ 创建表并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.0 p.pose.position.y = 0.85 p.pose.position.z = 0.40 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.5, 0.05)) print "-------------test4------------------" return p.pose def _add_grasp_block_(self, name): """ 创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.74 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) print "-------------test5------------------" return p.pose def _add_grasp_block2_(self, name): """ 创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.3 p.pose.position.z = 0.74 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) print "-------------test6------------------" return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. 使用抓握生成器生成动作来生成抓握; 基于moveit_simple_grasps pkg上的server_test.py示例 Generate the grasp Pose Array data for visualization, and then send the grasp goal to the grasp server. 生成抓取姿势阵列数据以进行可视化,然后将抓取目标发送到抓取服务器。 """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # 发送目标并等待结果: # 将目标发送到ActionServer,等待目标完成,并且必须抢占目标. state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # 发布掌握(用于调试/可视化目的): self._publish_grasps(grasps) print "-------------test7------------------" return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp 为该位置的姿势创建姿势数组数据 """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle ) place.place_pose.pose.orientation = Quaternion(*q) # 生成预安置方法: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.1 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.1 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) print "-------------test8------------------" return places def _create_pickup_goal(self, group, target, grasps): """ 创建一个MoveIt! 接送目标 创建一个捡起抓取物体的目标 """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # 配置目标计划选项: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 print "-------------test9------------------" return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal Create a place goal for MoveIt! """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 print "-------------test10------------------" return goal def _pickup(self, group, target, width): """ 使用计划小组选择目标 """ # 从掌握生成器服务器获取可能的掌握: grasps = self._generate_grasps(self._pose_coke_can, width) # 创建并发送提货目标: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False print "-------------test11------------------" return True def _place(self, group, target, place): """ 使用计划组放置目标 """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False print "-------------test12------------------" return True def _publish_grasps(self, grasps): """ 使用PoseArray消息将抓取发布为姿势 """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) print "-------------test13------------------" def _publish_places(self, places): """ 使用PoseArray消息将位置发布为姿势 """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg) print "-------------test14------------------"
class Pick_Place: def __init__(self, x, y, z): self._x = x self._y = y self._z = z # 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() #未知 self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name, self._x, self._y, self._z) rospy.sleep(0.5) # Define target place pose:定义目标位置姿势 self._pose_place = Pose() self._pose_place.position.x = 0 #self._pose_coke_can.position.x self._pose_place.position.y = -0.85 #self._pose_coke_can.position.y - 0.20 self._pose_place.position.z = 0.7 #self._pose_coke_can.position.z self._pose_place.orientation = Quaternion( *quaternion_from_euler(0.0, 0.0, 0.0)) self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 #self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) self._arm.set_named_target('up') self._arm.go(wait=True) print("up pose") print("第一部分恢复位置初始") # Create grasp generator 'generate' action client: # #创建抓取生成器“生成”动作客户端: print("开始Action通信") self._grasps_ac = SimpleActionClient( '/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown( 'Grasp generator action client not available!') return print("结束Action通信") # Create move group 'pickup' action client: # 创建移动组“抓取”操作客户端: print("开始pickup通信") self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return print("结束pickup通信") # Create move group 'place' action client: # 创建移动组“放置”动作客户端: print("开始place通信") self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return print("结束place通信") # Pick Coke can object:抓取快 while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(0.5) print("抓取物体") rospy.loginfo('Pick up successfully') print("pose_place: ", self._pose_place) # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(0.5) rospy.loginfo('Place successfully') def _generate_grasps(self, pose, width): """ 使用抓握生成器生成动作来生成抓握; 基于moveit_simple_grasps pkg上的server_test.py示例。 生成抓取姿势阵列数据以进行可视化, 然后将抓取目标发送到抓取服务器. """ self._grasps_ac.wait_for_server() rospy.loginfo("Successfully connected!") # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: # Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary. # rospy.loginfo("Sent goal,waiting,目标物体的位置:\n " + str(goal)) #self._grasps_ac.wait_for_result() #发送目标并等待结果: # #将目标发送到ActionServer,等待目标完成,然后抢占目标是必要的。 t_start = rospy.Time.now() state = self._grasps_ac.send_goal_and_wait(goal) t_end = rospy.Time.now() t_toal = t_end - t_start rospy.loginfo("发送时间Result received in " + str(t_toal.to_sec())) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) """ print("---------grasps--start------------") print(grasps) print("-----------test-grasps-----end-----------------") rospy.sleep(2) print("-grasps---sleep----end-----") """ return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp 为该位置的姿势创建姿势数组数据 """ # Generate places: places = [] now = rospy.Time.now() #print("---------------test3--------------------------") for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach:生成前置位置方法: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame( ) place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.1 #print("place1---test=====") # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame( ) place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.06 #print("place2---test=====") # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) """ print(places) print("------test4---------------------------------------") """ return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal Create a goal for picking up the grasping object创建一个捡起抓取物体的目标 """ # Create goal: goal = PickupGoal() goal.group_name = group #规划的组 goal.target_name = target #要拾取的对象的名称( #可能使用的抓握列表。 必须至少填写一个掌握 goal.possible_grasps.extend(grasps) #障碍物的可选清单,我们掌握有关语义的信息,可以在抓取过程中触摸/推动/移动这些障碍物; 注意:如果使用对象名称“ all”,则在进近和提升过程中禁止与所有对象发生碰撞。 goal.allowed_touch_objects.append(target) #如果没有可用的名称,则在碰撞图中支撑表面(例如桌子)的名称可以留空 goal.support_surface_name = self._table_object_name # Configure goal planning options:配置目标计划选项: #运动计划者可以规划的最长时间 goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 """ print("----goal-------test-----") print(goal) print("----goal---end------test---") """ rospy.sleep(0.5) print("-goal---sleep----end-----") return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal Create a place goal for MoveIt! """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _add_grasp_block_(self, name, _x, _y, _z): """ Create and add block to the scene创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = _x p.pose.position.y = _y p.pose.position.z = _z q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. #self._scene.add_box(name, p, (0.045, 0.045, 0.045)) return p.pose def _pickup(self, group, target, width): """ Pick up a target using the planning group使用规划小组选择目标 """ # Obtain possible grasps from the grasp generator server:从掌握生成器服务器获取可能的抓握: grasps = self._generate_grasps(self._pose_coke_can, width) #print("--goal--start----") # Create and send Pickup goal:创建并发送zhua取目标: goal = self._create_pickup_goal(group, target, grasps) #print("--goal--end----") #print("---pick---up---1----") state = self._pickup_ac.send_goal_and_wait(goal) #print("-----------------state-------------------") #print(state) #print("---pick---up---2----") if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() """ print("-----test1--------") print(result) print("------test1--end------") """ # Check for error: err = result.error_code.val print(err) if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False print("pickup-----end-----") rospy.sleep(0.5) return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places:获取可能的位置: places = self._generate_places(place) # Create and send Place goal:创建并发送地方目标: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() """ print("-----test2--------") print(result) print("------test2--end------") """ # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message使用PoseArray消息将抓取发布为姿势 """ print(self._grasps_pub.get_num_connections()) if self._grasps_pub.get_num_connections() != 1: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) print(msg) rospy.loginfo('Publisher ' + str(len(msg.poses)) + ' poses') self._grasps_pub.publish(msg) """ print("11-111") rospy.loginfo('Publisher'+str(len(msg))+'poses') print("11-111") rospy.sleep(2) """ def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg)
def execute_inner(self, userdata): global ms self.last_mv = rospy.Time.now() rospy.loginfo('Beginning Exploration') client = SimpleActionClient('explore_server', ExploreTaskAction) rospy.loginfo('WAITING FOR EXPLORE SERVER...') client.wait_for_server() rospy.loginfo('EXPLORE SERVER IS NOW UP!') boundary = PolygonStamped() boundary.header.frame_id = "map" boundary.header.stamp = rospy.Time.now() r = userdata.boundary / 2.0 rospy.loginfo('boundary : {}'.format(r)) x, y, _ = userdata.initial_point boundary.polygon.points.append(Point(x + r, y + r, 0)) boundary.polygon.points.append(Point(x - r, y + r, 0)) boundary.polygon.points.append(Point(x - r, y - r, 0)) boundary.polygon.points.append(Point(x + r, y - r, 0)) center = PointStamped() center.header.frame_id = "map" center.header.stamp = rospy.Time.now() center.point.x = x center.point.y = y center.point.z = 0.0 goal = ExploreTaskGoal() goal.explore_boundary = boundary goal.explore_center = center client.send_goal(goal) while True: if rospy.is_shutdown(): exit() if client.wait_for_result( rospy.Duration(0.3)): # 0.3 sec. timeout to check for cans # The exploration node has finished res = client.get_state() rospy.loginfo('EXPLORE SERVER STATE:{}'.format(res)) if res == 3: ## SUCCEEDED # if exploration is complete... userdata.boundary += 1.0 # explore a larger area return 'succeeded' # finished! yay! else: print client.get_goal_status_text() print 'explore server failed : {}'.format(res) # when explore server gives up, can't explore return 'stuck' now = rospy.Time.now() if self.objective == 'discovery': t = ms.dis_data.time p = ms.dis_pt() elif self.objective == 'delivery': t = ms.del_data.time p = ms.del_pt() else: rospy.logerr('Invalid objective passed to Explore state') return 'aborted' # if we're here, exploration is not complete yet... if t != None: # check initialized dt = (now - t).to_sec() discovered = (dt < 2.0) # last seen within the last 2 seconds if discovered: # if can was found ... client.cancel_all_goals() userdata.destination = p return 'discovered' # more than 20 seconds have passed while completely still # we're probably stuck if (now - self.last_mv).to_sec() > 20.0: print 'haven\'t been moving for a while!' client.cancel_all_goals() return 'stuck' # bad name... "stuck" would be better
class PigeonPickAndPlace: #Class constructor (parecido com java, inicializa o que foi instanciado) def __init__(self): # Retrieve params: self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016) self._arm_group = rospy.get_param('~arm', 'arm_move_group') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) # Create planning scene where we will add the objects etc. self._scene = PlanningSceneInterface() # Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene (remove the old objects: self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: # TODO get the position of the detected object self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name) rospy.sleep(1.0) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Pick object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') def __del__(self): # Clean the scene self._scene.remove_world_object(self._grasp_object_name) def _add_object_grasp(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() #pose p.header.stamp = rospy.Time.now() rospy.loginfo(self._robot.get_planning_frame()) p.pose.position.x = 0.11 # 0.26599 # antigo = 0.75 - 0.01 p.pose.position.y = -0.31 # -0.030892 #antigo = 0.25 - 0.01 p.pose.position.z = 0.41339 #antigo = 1.00 + (0.3 + 0.03) / 2.0 #p.pose.orientation.x = 0.0028925 #p.pose.orientation.y = -0.0019311 #p.pose.orientation.z = -0.71058 #p.pose.orientation.w = 0.70361 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/, # using the measure tape tool from meshlab. # The box is the bounding box of the lego cylinder. # The values are taken from the cylinder base diameter and height. # the size is length (x),thickness(y),height(z) # I don't know if the detector return this values of object self._scene.add_box(name, p, (0.032, 0.016, 0.020)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps #rospy.logerr('Generated: %f grasps:' % grasps.size) # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) #goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 5.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 10 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_object_grasp, 0.016) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg)