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)