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 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)
