def generateConstraint(self):
""" PositionConstraint """
entity_id_area_name_map = {}
for desig, area_name in self.entity_designator_area_name_map.iteritems():
entity = desig.resolve()
try:
area_name = area_name.resolve()
except:
pass
if entity:
entity_id_area_name_map[entity.id] = area_name
else:
rospy.logerr("Designator {0} in entity_designator_area_name_map resolved to {1}.".format(desig, entity))
# Put a None item in the dict. We check on that and if there's a None, something failed.
entity_id_area_name_map[entity] = area_name
if None in entity_id_area_name_map:
rospy.logerr("At least 1 designator in self.entity_designator_area_name_map failed")
return None
pc = self.robot.ed.navigation.get_position_constraint(entity_id_area_name_map)
# Orientation constraint is the entity itself...
entity_lookat = self.entity_lookat_designator.resolve()
if not entity_lookat:
rospy.logerr("Could not resolve entity_lookat_designator".format(self.entity_lookat_designator))
return None
look_at = kdl_vector_to_point_msg(entity_lookat.pose.extractVectorStamped().vector)
oc = OrientationConstraint(look_at=look_at, frame=entity_lookat.pose.frame_id)
return pc, oc
def get_entities(self,
type="",
center_point=VectorStamped(),
radius=0,
id="",
parse=True):
self._publish_marker(center_point, radius)
center_point_in_map = center_point.projectToFrame(
"/map", self._tf_listener)
query = SimpleQueryRequest(id=id,
type=type,
center_point=kdl_vector_to_point_msg(
center_point_in_map.vector),
radius=radius)
try:
entity_infos = self._ed_simple_query_srv(query).entities
entities = map(from_entity_info, entity_infos)
except Exception, e:
rospy.logerr(
"ERROR: robot.ed.get_entities(id=%s, type=%s, center_point=%s, radius=%s)"
% (id, type, str(center_point), str(radius)))
rospy.logerr("L____> [%s]" % e)
return []
def get_entities(self, type="", center_point=VectorStamped(), radius=float('inf'), id="", ignore_z=False):
"""
Get entities via Simple Query interface
:param type: Type of entity
:param center_point: Point from which radius is measured
:param radius: Distance between center_point and entity
:param id: ID of entity
:param ignore_z: Consider only the distance in the X,Y plane for the radius from center_point
"""
self._publish_marker(center_point, radius)
center_point_in_map = center_point.projectToFrame("/map", self.tf_listener)
query = SimpleQueryRequest(id=id, type=type, center_point=kdl_vector_to_point_msg(center_point_in_map.vector),
radius=radius, ignore_z=ignore_z)
try:
entity_infos = self._ed_simple_query_srv(query).entities
entities = map(from_entity_info, entity_infos)
except Exception as e:
rospy.logerr("ERROR: robot.ed.get_entities(id={}, type={}, center_point={}, radius={}, ignore_z={})".format(
id, type, str(center_point), str(radius), ignore_z))
rospy.logerr("L____> [%s]" % e)
return []
return entities
def _turn_towards_operator(self):
f = self._robot.base.get_location().frame
robot_position = f.p
operator_position = self._last_operator._pose.p
p = PositionConstraint()
p.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2"% (operator_position.x(), operator_position.y(), self._operator_radius)
o = OrientationConstraint()
if self._operator_id:
o.frame = self._operator_id
else:
o.frame = 'map'
o.look_at = kdl_conversions.kdl_vector_to_point_msg(self._last_operator.pose.frame.p)
dx = operator_position.x() - robot_position.x()
dy = operator_position.y() - robot_position.y()
yaw = math.atan2(dy, dx)
# ToDo: make nice!
pose = geometry_msgs.msg.PoseStamped()
pose.header.frame_id = "/map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = robot_position.x()
pose.pose.position.y = robot_position.y()
xx, yy, zz, ww = kdl.Rotation.RPY(0, 0, yaw).GetQuaternion()
pose.pose.orientation.x = xx
pose.pose.orientation.y = yy
pose.pose.orientation.z = zz
pose.pose.orientation.w = ww
plan = [pose]
print "Operator within self._lookat_radius"
self._robot.base.local_planner.setPlan(plan, p, o)
def generate_constraint(robot, entity_designator_area_name_map,
entity_lookat_designator):
""" Staticmethod generating the position and orientation constraint.
By implementing this as a staticmethod, it can also be used for other purposes.
:param robot: robot object
:param entity_designator_area_name_map: dictionary mapping EdEntityDesignators to a string or designator
resolving to a string, representing the area, e.g., entity_designator_area_name_map[<EdEntity>] = 'in_front_of'.
:param entity_lookat_designator: EdEntityDesignator defining the entity the robot should look at. This is used
to compute the orientation constraint.
:return: (tuple(PositionConstraint, OrientationConstraint)). If one of the entities does not resolve,
None is returned.
"""
entity_id_area_name_map = {}
for desig, area_name in entity_designator_area_name_map.iteritems():
entity = desig.resolve()
try:
area_name = area_name.resolve()
except:
pass
if entity:
entity_id_area_name_map[entity.id] = area_name
else:
rospy.logerr(
"Designator {0} in entity_designator_area_name_map resolved to {1}."
.format(desig, entity))
# Put a None item in the dict. We check on that and if there's a None, something failed.
entity_id_area_name_map[entity] = area_name
if None in entity_id_area_name_map:
rospy.logerr(
"At least 1 designator in self.entity_designator_area_name_map failed"
)
return None
rospy.logdebug(
"Navigating to symbolic {}".format(entity_id_area_name_map))
pc = robot.ed.navigation.get_position_constraint(
entity_id_area_name_map)
# Orientation constraint is the entity itself...
entity_lookat = entity_lookat_designator.resolve()
if not entity_lookat:
rospy.logerr("Could not resolve entity_lookat_designator".format(
entity_lookat_designator))
return None
look_at = kdl_vector_to_point_msg(
entity_lookat.pose.extractVectorStamped().vector)
oc = OrientationConstraint(look_at=look_at,
frame=entity_lookat.pose.frame_id)
return pc, oc
def _visualize_breadcrumbs(self):
breadcrumbs_msg = Marker()
breadcrumbs_msg.type = Marker.POINTS
breadcrumbs_msg.scale.x = 0.05
breadcrumbs_msg.header.stamp = rospy.get_rostime()
breadcrumbs_msg.header.frame_id = "/map"
breadcrumbs_msg.color.a = 1
breadcrumbs_msg.color.r = 0
breadcrumbs_msg.color.g = 1
breadcrumbs_msg.color.b = 1
breadcrumbs_msg.lifetime = rospy.Time(1.0)
breadcrumbs_msg.id = 0
breadcrumbs_msg.action = Marker.ADD
for crumb in self._breadcrumbs:
breadcrumbs_msg.points.append(kdl_conversions.kdl_vector_to_point_msg(crumb.pose.frame.p))
self._breadcrumb_pub.publish(breadcrumbs_msg)
def generate_constraint(robot, entity_designator_area_name_map, entity_lookat_designator):
""" Staticmethod generating the position and orientation constraint.
By implementing this as a staticmethod, it can also be used for other purposes.
:param robot: robot object
:param entity_designator_area_name_map: dictionary mapping EdEntityDesignators to a string or designator
resolving to a string, representing the area, e.g., entity_designator_area_name_map[<EdEntity>] = 'in_front_of'.
:param entity_lookat_designator: EdEntityDesignator defining the entity the robot should look at. This is used
to compute the orientation constraint.
:return: (tuple(PositionConstraint, OrientationConstraint)). If one of the entities does not resolve,
None is returned.
"""
entity_id_area_name_map = {}
for desig, area_name in entity_designator_area_name_map.iteritems():
entity = desig.resolve()
try:
area_name = area_name.resolve()
except:
pass
if entity:
entity_id_area_name_map[entity.id] = area_name
else:
rospy.logerr("Designator {0} in entity_designator_area_name_map resolved to {1}.".format(desig, entity))
# Put a None item in the dict. We check on that and if there's a None, something failed.
entity_id_area_name_map[entity] = area_name
if None in entity_id_area_name_map:
rospy.logerr("At least 1 designator in self.entity_designator_area_name_map failed")
return None
rospy.logdebug("Navigating to symbolic {}".format(entity_id_area_name_map))
pc = robot.ed.navigation.get_position_constraint(entity_id_area_name_map)
# Orientation constraint is the entity itself...
entity_lookat = entity_lookat_designator.resolve()
if not entity_lookat:
rospy.logerr("Could not resolve entity_lookat_designator".format(entity_lookat_designator))
return None
look_at = kdl_vector_to_point_msg(entity_lookat.pose.extractVectorStamped().vector)
oc = OrientationConstraint(look_at=look_at, frame=entity_lookat.pose.frame_id)
return pc, oc
def _visualize_breadcrumbs(self):
breadcrumbs_msg = Marker()
breadcrumbs_msg.type = Marker.POINTS
breadcrumbs_msg.scale.x = 0.05
breadcrumbs_msg.header.stamp = rospy.get_rostime()
breadcrumbs_msg.header.frame_id = "/map"
breadcrumbs_msg.color.a = 1
breadcrumbs_msg.color.r = 0
breadcrumbs_msg.color.g = 1
breadcrumbs_msg.color.b = 1
breadcrumbs_msg.lifetime = rospy.Time(1.0)
breadcrumbs_msg.id = 0
breadcrumbs_msg.action = Marker.ADD
for crumb in self._breadcrumbs:
breadcrumbs_msg.points.append(
kdl_conversions.kdl_vector_to_point_msg(crumb.pose.frame.p))
self._breadcrumb_pub.publish(breadcrumbs_msg)
def look_at_constraint(entity_lookat_designator, offset=0.0):
"""
Generate navigation constraints for looking at an entity
:param entity_lookat_designator: EdEntityDesignator defining the entity the robot should look at.
:param offset: offset the angle with respect to the entity
:return: navigation constraints. If the entity does not resolve, None is returned.
:rtype: tuple(PositionConstraint, OrientationConstraint)
"""
# Orientation constraint is the entity itself...
entity_lookat = entity_lookat_designator.resolve()
if not entity_lookat:
rospy.logerr("Could not resolve entity_lookat_designator".format(
entity_lookat_designator))
return None
look_at = kdl_vector_to_point_msg(
entity_lookat.pose.extractVectorStamped().vector)
pc = None
oc = OrientationConstraint(look_at=look_at,
angle_offset=offset,
frame=entity_lookat.pose.frame_id)
return pc, oc
def _turn_towards_operator(self):
f = self._robot.base.get_location().frame
robot_position = f.p
operator_position = self._last_operator._pose.p
p = PositionConstraint()
p.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (operator_position.x(),
operator_position.y(),
self._operator_radius)
o = OrientationConstraint()
if self._operator_id:
o.frame = self._operator_id
else:
o.frame = 'map'
o.look_at = kdl_conversions.kdl_vector_to_point_msg(
self._last_operator.pose.frame.p)
dx = operator_position.x() - robot_position.x()
dy = operator_position.y() - robot_position.y()
yaw = math.atan2(dy, dx)
# ToDo: make nice!
pose = geometry_msgs.msg.PoseStamped()
pose.header.frame_id = "/map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = robot_position.x()
pose.pose.position.y = robot_position.y()
xx, yy, zz, ww = kdl.Rotation.RPY(0, 0, yaw).GetQuaternion()
pose.pose.orientation.x = xx
pose.pose.orientation.y = yy
pose.pose.orientation.z = zz
pose.pose.orientation.w = ww
plan = [pose]
print "Operator within self._lookat_radius"
self._robot.base.local_planner.setPlan(plan, p, o)
def _update_navigation(self):
"""Set the navigation plan to match the breadcrumbs collected into self._breadcrumbs.
This list has all the Entity's of where the operator has been"""
self._robot.head.cancel_goal()
f = self._robot.base.get_location().frame
robot_position = f.p
operator_position = self._last_operator._pose.p
''' Define end goal constraint, solely based on the (old) operator position '''
p = PositionConstraint()
p.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (operator_position.x(),
operator_position.y(),
self._operator_radius)
o = OrientationConstraint()
if self._operator_id:
o.frame = self._operator_id
else:
o.frame = 'map'
o.look_at = kdl_conversions.kdl_vector_to_point_msg(
self._last_operator.pose.frame.p)
''' Calculate global plan from robot position, through breadcrumbs, to the operator '''
res = 0.05
kdl_plan = []
previous_point = robot_position
if self._operator:
breadcrumbs = self._breadcrumbs + [self._operator]
else:
breadcrumbs = self._breadcrumbs + [self._last_operator]
for crumb in breadcrumbs:
assert isinstance(crumb, Entity)
diff = crumb._pose.p - previous_point
dx, dy = diff.x(), diff.y()
length = crumb.distance_to_2d(previous_point)
if length != 0:
dx_norm = dx / length
dy_norm = dy / length
yaw = math.atan2(dy, dx)
start = 0
end = int(length / res)
for i in range(start, end):
x = previous_point.x() + i * dx_norm * res
y = previous_point.y() + i * dy_norm * res
kdl_plan.append(
kdl_conversions.kdl_frame_stamped_from_XYZRPY(x=x,
y=y,
z=0,
yaw=yaw))
previous_point = copy.deepcopy(crumb._pose.p)
# Delete the elements from the plan within the operator radius from the robot
cutoff = int(self._operator_radius / (2.0 * res))
if len(kdl_plan) > cutoff:
del kdl_plan[-cutoff:]
ros_plan = frame_stampeds_to_pose_stampeds(kdl_plan)
# Check if plan is valid. If not, remove invalid points from the path
if not self._robot.base.global_planner.checkPlan(ros_plan):
print "Breadcrumb plan is blocked, removing blocked points"
# Go through plan from operator to robot and pick the first unoccupied point as goal point
ros_plan = [
point for point in ros_plan
if self._robot.base.global_planner.checkPlan([point])
]
self._visualize_plan(ros_plan)
self._robot.base.local_planner.setPlan(ros_plan, p, o)
def _update_navigation(self):
"""Set the navigation plan to match the breadcrumbs collected into self._breadcrumbs.
This list has all the Entity's of where the operator has been"""
self._robot.head.cancel_goal()
f = self._robot.base.get_location().frame
robot_position = f.p
operator_position = self._last_operator._pose.p
''' Define end goal constraint, solely based on the (old) operator position '''
p = PositionConstraint()
p.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2"% (operator_position.x(), operator_position.y(),
self._operator_radius)
o = OrientationConstraint()
if self._operator_id:
o.frame = self._operator_id
else:
o.frame = 'map'
o.look_at = kdl_conversions.kdl_vector_to_point_msg(self._last_operator.pose.frame.p)
''' Calculate global plan from robot position, through breadcrumbs, to the operator '''
res = 0.05
kdl_plan = []
previous_point = robot_position
if self._operator:
breadcrumbs = self._breadcrumbs + [self._operator]
else:
breadcrumbs = self._breadcrumbs + [self._last_operator]
for crumb in breadcrumbs:
assert isinstance(crumb, Entity)
diff = crumb._pose.p - previous_point
dx, dy = diff.x(), diff.y()
length = crumb.distance_to_2d(previous_point)
if length != 0:
dx_norm = dx / length
dy_norm = dy / length
yaw = math.atan2(dy, dx)
start = 0
end = int(length / res)
for i in range(start, end):
x = previous_point.x() + i * dx_norm * res
y = previous_point.y() + i * dy_norm * res
kdl_plan.append(kdl_conversions.kdl_frame_stamped_from_XYZRPY(x=x, y=y, z=0, yaw=yaw))
previous_point = copy.deepcopy(crumb._pose.p)
# Delete the elements from the plan within the operator radius from the robot
cutoff = int(self._operator_radius/(2.0*res))
if len(kdl_plan) > cutoff:
del kdl_plan[-cutoff:]
ros_plan = frame_stampeds_to_pose_stampeds(kdl_plan)
# Check if plan is valid. If not, remove invalid points from the path
if not self._robot.base.global_planner.checkPlan(ros_plan):
print "Breadcrumb plan is blocked, removing blocked points"
# Go through plan from operator to robot and pick the first unoccupied point as goal point
ros_plan = [point for point in ros_plan if self._robot.base.global_planner.checkPlan([point])]
self._visualize_plan(ros_plan)
self._robot.base.local_planner.setPlan(ros_plan, p, o)