def __init__(self, topic='motion/controller/look_at/in_pose_ref', duration=120, interval=[3, 5], maxXYZ=[1, 0.3, 0.5], minXYZ=[1.0, -0.3, 0.0], frame_xyz='base_link', frame_out='odom_combined'): super(RandPoseGenerator, self).__init__(outcomes=['done', 'failure']) # Store topic parameter for later use. if not isinstance(topic, str): raise ValueError("Topic name must be string.") if not isinstance(duration, (int, float)) or duration <= 0: raise ValueError("Duration must be positive number.") if len(interval) != 2 or any([ not isinstance(t, (int, float)) for t in interval ]) or any([t < 0 for t in interval]) or interval[0] > interval[1]: raise ValueError("Interval must represent interval of time.") if len(minXYZ) != 3 or any( [not isinstance(v, (int, float)) for v in minXYZ]): raise ValueError("minXYZ: list of three numbers was expected.") if len(maxXYZ) != 3 or any( [not isinstance(v, (int, float)) for v in maxXYZ]): raise ValueError("maxXYZ: list of three numbers was expected.") if not isinstance(frame_xyz, str) or not isinstance(frame_out, str): raise ValueError("Frame names must be string.") self._topic = topic self._duration = Duration.from_sec(duration) self._interval = interval self._minXYZ = minXYZ self._maxXYZ = maxXYZ self._frame_in = frame_xyz self._frame_out = frame_out # create proxies self._publisher = ProxyPublisher({self._topic: PoseStamped}) self._tf_listener = ProxyTransformListener().listener() self._tf_listener.waitForTransform(self._frame_out, self._frame_in, rospy.Time(), rospy.Duration(1)) if not self._tf_listener.canTransform(self._frame_out, self._frame_in, rospy.Time(0)): raise ValueError( "Unable to perform transform between frames %s and %s." % (self._frame_out, self._frame_in)) # state self._start_timestamp = None self._movement_timestamp = None self._movement_duration = Duration() # error in enter hook self._error = False
def __init__(self): super(BrohoofSM, self).__init__() self.name = 'Brohoof' # parameters of this behavior self.add_parameter('wait_time', 5) self.add_parameter('neck_angle', 0.25) self.add_parameter('brohoof_cone', 0.78) self.add_parameter('brohoof_upper_limit', 0.30) # references to used behaviors # Additional initialization code can be added inside the following tags # [MANUAL_INIT] self._tf = ProxyTransformListener().listener()
def __init__(self, choice): ''' Constructor ''' super(GetPipePose, self).__init__(outcomes=['succeeded'], input_keys = ['centerpose'], output_keys = ['pipepose']) self._choice = choice self._tf = ProxyTransformListener().listener() self._succeeded = False
def __init__(self): ''' Get tf proxy and neessary transform. :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise ''' # get transform listener singlenton self._tf = ProxyTransformListener().listener() # wait for pose publication self._tf.waitForTransform('bone51', 'base_link', rospy.Time(), rospy.Duration(10.0)) # get neck base translation vector in base_link frame (trans, rot) = self._tf.lookupTransform('base_link', 'bone51', rospy.Time()) self._t5051 = trans # get translation from bone51 to bone52 (trans, rot) = self._tf.lookupTransform('bone51', 'bone52', rospy.Time()) self._t5152 = trans
def __init__(self): super(DetectPipesSM, self).__init__() self.name = 'Detect Pipes' # parameters of this behavior self.add_parameter('pose_offset', 0.3) self.add_parameter('close_offset', 0.1) # references to used behaviors # Additional initialization code can be added inside the following tags # [MANUAL_INIT] self._tf = ProxyTransformListener().listener()
class GetPipePose(EventState): ''' ># centerpose PoseStamped() Pose of centered pipe. #> pipepose PoseStamped() Pose of desired pipe. ''' TOP_LEFT = 1 TOP_RIGHT = 2 CENTER = 3 DOWN_LEFT = 4 DOWN_RIGHT = 5 def __init__(self, choice): ''' Constructor ''' super(GetPipePose, self).__init__(outcomes=['succeeded'], input_keys = ['centerpose'], output_keys = ['pipepose']) self._choice = choice self._tf = ProxyTransformListener().listener() self._succeeded = False def execute(self, userdata): ''' Execute this state ''' if self._succeeded: return 'succeeded' def on_enter(self, userdata): tempPose = PoseStamped() tempPose = self._tf.transformPose('base_link', userdata.centerpose) if self._choice == GetPipePose.TOP_LEFT: tempPose.pose.position.y = tempPose.pose.position.y + 0.105 tempPose.pose.position.z = tempPose.pose.position.z + 0.105 if self._choice == GetPipePose.TOP_RIGHT: tempPose.pose.position.y = tempPose.pose.position.y - 0.105 tempPose.pose.position.z = tempPose.pose.position.z + 0.105 if self._choice == GetPipePose.DOWN_RIGHT: tempPose.pose.position.y = tempPose.pose.position.y - 0.105 tempPose.pose.position.z = tempPose.pose.position.z - 0.105 if self._choice == GetPipePose.DOWN_LEFT: tempPose.pose.position.y = tempPose.pose.position.y + 0.105 tempPose.pose.position.z = tempPose.pose.position.z - 0.105 userdata.pipepose = tempPose self._succeeded = True
def __init__(self, move_group, ignore_collisions = False): ''' Constructor ''' super(CalculateCartesianPathState, self).__init__(outcomes=['planned', 'failed'], input_keys=['eef_pose'], output_keys=['joint_trajectory', 'plan_fraction']) self._topic = '/compute_cartesian_path' self._srv = ProxyServiceCaller({self._topic: GetCartesianPath}) self._tf = ProxyTransformListener().listener() self._move_group = move_group self._result = None self._failed = False self._ignore_collisions = ignore_collisions
class CalculateCartesianPathState(EventState): ''' Calculates the trajectory for a cartesian path to the given goal endeffector pose. -- move_group string Name of the move group to be used for planning. -- ignore_collisions bool Ignore collision, use carefully. ># eef_pose PoseStamped Target pose of the endeffector. #> joint_trajectory JointTrajectory Calculated joint trajectory. #> plan_fraction float Fraction of the planned path. <= planned Found a plan to the target. <= failed Failed to find a plan to the given target. ''' def __init__(self, move_group, ignore_collisions = False): ''' Constructor ''' super(CalculateCartesianPathState, self).__init__(outcomes=['planned', 'failed'], input_keys=['eef_pose'], output_keys=['joint_trajectory', 'plan_fraction']) self._topic = '/compute_cartesian_path' self._srv = ProxyServiceCaller({self._topic: GetCartesianPath}) self._tf = ProxyTransformListener().listener() self._move_group = move_group self._result = None self._failed = False self._ignore_collisions = ignore_collisions def execute(self, userdata): ''' Execute this state ''' if self._failed: return 'failed' if self._result.error_code.val == MoveItErrorCodes.SUCCESS and self._result.fraction > .2: userdata.plan_fraction = self._result.fraction userdata.joint_trajectory = self._result.solution.joint_trajectory Logger.loginfo('Successfully planned %.2f of the path' % self._result.fraction) return 'planned' elif self._result.error_code.val == MoveItErrorCodes.SUCCESS: Logger.logwarn('Only planned %.2f of the path' % self._result.fraction) self._failed = True return 'failed' else: Logger.logwarn('Failed to plan trajectory: %d' % self._result.error_code.val) self._failed = True return 'failed' def on_enter(self, userdata): self._failed = False request = GetCartesianPathRequest() request.group_name = self._move_group request.avoid_collisions = not self._ignore_collisions request.max_step = 0.05 request.header = userdata.eef_pose.header request.waypoints.append(userdata.eef_pose.pose) now = rospy.Time.now() try: self._tf.waitForTransform('base_link', 'gripper_cam_link', now, rospy.Duration(1)) (p,q) = self._tf.lookupTransform('gripper_cam_link', 'base_link', now) c = OrientationConstraint() c.header.frame_id = 'base_link' c.header.stamp = now c.link_name = 'gripper_cam_link' c.orientation.x = q[0] c.orientation.y = q[1] c.orientation.z = q[2] c.orientation.w = q[3] c.weight = 1 c.absolute_x_axis_tolerance = 0.1 c.absolute_y_axis_tolerance = 0.1 c.absolute_z_axis_tolerance = 0.1 #request.path_constraints.orientation_constraints.append(c) self._result = self._srv.call(self._topic, request) except Exception as e: Logger.logwarn('Exception while calculating path:\n%s' % str(e)) self._failed = True
def __init__(self, detection_topic='detection', label='*', type='*', exit_states=['no_detections'], pose_topic='', pose_frame_id='odom_combined', detection_period=1.0): super(ObjectDetectionMonitor, self).__init__(outcomes=[ 'no_detections', 'have_detections', 'detection_matches', 'failure' ], output_keys=['pose']) # store state parameter for later use. self._detection_topic = detection_topic self._label = label self._type = type self._pose_topic = pose_topic self._frame_id = pose_frame_id self._exit_states = exit_states self._detection_period = detection_period # setup proxies self._detection_subscriber = ProxySubscriberCached( {self._detection_topic: DetectionArray}) self._detection_subscriber.enable_buffer(self._detection_topic) if self._pose_topic: self._pose_publisher = ProxyPublisher( {self._pose_topic: PoseStamped}) else: self._pose_publisher = None self._tf_listener = ProxyTransformListener().listener() # check parameters if not isinstance( self._frame_id, str): # or not self._tf_listener.frameExists(self._frame_id): raise ValueError( 'ObjectDetectionMonitor: %s is not a valid frame' % self._frame_id) if not isinstance(self._label, str) or not isinstance(self._type, str): raise ValueError( 'ObjectDetectionMonitor: label and type parameters must be string.' ) if not isinstance(self._detection_period, float) or self._detection_period < 0.0: raise ValueError( 'ObjectDetectionMonitor: detection_period must be float.') if not isinstance(self._exit_states, list) or not all([ state in ['no_detections', 'have_detections', 'detection_matches'] for state in self._exit_states ]): raise ValueError( 'ObjectDetectionMonitor: incorrect list of exit states.') # pose buffers self._pose = None self._last_detection_stamp = None self._last_match_stamp = None self._last_match_id = None
class ObjectDetectionMonitor(EventState): ''' Receive list of detected objects as cob_object_detection_msgs/DetectionArray messages and check if there is an object which matches given (label, type) pair. Based on present of this object classify situation in `no_detections` state, `have_detections` (there is objects but none of them matches give (label, type)), `detection_matches` (desired object found). In the last case its pose bublished on given topic. -- detection_topic string DetectionArray message topic. -- label string Object label. Use '*' to match all labals. -- type string Object type. Use '*' to match all types. -- detection_period float Parameteres: detection_period (s), position_tolerance (m), orientation_tolerance (rad). -- exit_states string[] Stop monitoring and return outcome if detected situation in this list. -- pose_topic string Topic for publishing matched object pose (may be Empty). -- pose_frame_id string Frame in which pose should be published. #> pose object Object pose before exit. <= no_detections Nothing have been detected for detection_period. <= have_detections Objects were detected, but not of them matches (label, type) <= detecton_matches Object presents and it is moving. `pose` key contains last pose. <= failure Runtime error. ''' def __init__(self, detection_topic='detection', label='*', type='*', exit_states=['no_detections'], pose_topic='', pose_frame_id='odom_combined', detection_period=1.0): super(ObjectDetectionMonitor, self).__init__(outcomes=[ 'no_detections', 'have_detections', 'detection_matches', 'failure' ], output_keys=['pose']) # store state parameter for later use. self._detection_topic = detection_topic self._label = label self._type = type self._pose_topic = pose_topic self._frame_id = pose_frame_id self._exit_states = exit_states self._detection_period = detection_period # setup proxies self._detection_subscriber = ProxySubscriberCached( {self._detection_topic: DetectionArray}) self._detection_subscriber.enable_buffer(self._detection_topic) if self._pose_topic: self._pose_publisher = ProxyPublisher( {self._pose_topic: PoseStamped}) else: self._pose_publisher = None self._tf_listener = ProxyTransformListener().listener() # check parameters if not isinstance( self._frame_id, str): # or not self._tf_listener.frameExists(self._frame_id): raise ValueError( 'ObjectDetectionMonitor: %s is not a valid frame' % self._frame_id) if not isinstance(self._label, str) or not isinstance(self._type, str): raise ValueError( 'ObjectDetectionMonitor: label and type parameters must be string.' ) if not isinstance(self._detection_period, float) or self._detection_period < 0.0: raise ValueError( 'ObjectDetectionMonitor: detection_period must be float.') if not isinstance(self._exit_states, list) or not all([ state in ['no_detections', 'have_detections', 'detection_matches'] for state in self._exit_states ]): raise ValueError( 'ObjectDetectionMonitor: incorrect list of exit states.') # pose buffers self._pose = None self._last_detection_stamp = None self._last_match_stamp = None self._last_match_id = None def on_enter(self, userdata): # reset pose buffers self._pose = None stamp = rospy.Time.now() self._last_detection_stamp = stamp self._last_match_stamp = stamp self._last_match_id = None Logger.loginfo('ObjectDetectionMonitor: start monitoring.') def execute(self, userdata): # list of found matches have_detections = False best_match = None match = None # check if new message is available while self._detection_subscriber.has_buffered(self._detection_topic): # get detections msg detections_msg = self._detection_subscriber.get_from_buffer( self._detection_topic) # check if have detections if len(detections_msg.detections) > 0: have_detections = True # process detections for detection in detections_msg.detections: # check match is_match = True if self._label != '*' and detection.label != self._label: is_match = False if self._type != '*' and detection.type != self._type: is_match = False # register match if is_match: match = detection if match.id == self._last_match_id: best_match = match if best_match: match = best_match # result stamp = rospy.Time.now() if not have_detections: # no object detected # check if detection preiod exceeded if (stamp - self._last_detection_stamp ).to_sec() > self._detection_period: # no detections state if 'no_detections' in self._exit_states: Logger.loginfo('ObjectDetectionMonitor: no detections.') return 'no_detections' elif not match: # object detected but it is not match self._last_detection_stamp = stamp # check if detection perod exceeded if (stamp - self._last_detection_stamp ).to_sec() > self._detection_period: # have_detections state if 'have_detections' in self._exit_states: Logger.loginfo('ObjectDetectionMonitor: detection.') return 'have_detections' else: # appropriate match is found self._last_match_stamp = stamp self._last_match_id = match.id # publish if self._pose_publisher: # transform to frame_id try: # transform pose_stamped = PoseStamped( pose=match.pose, header=Header(frame_id=match.header.frame_id)) self._pose = self._tf_listener.transformPose( self._frame_id, pose_stamped) except tf.Exception as e: Logger.logwarn( 'Unable to transform from %s to %s' % (match.pose.header.frame_id, self._frame_id)) return 'failure' # publish self._pose_publisher.publish(self._pose_topic, self._pose) # match_detection state if 'detection_matches' in self._exit_states: Logger.loginfo('ObjectDetectionMonitor: match.') return 'detection_matches' return None def on_exit(self, userdata): userdata.pose = self._pose
class HeadIK: ''' Implements SweetieBot head and eyes invese kinematics. Meant to be run in FlexBe state enviroment. ''' def __init__(self): ''' Get tf proxy and neessary transform. :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise ''' # get transform listener singlenton self._tf = ProxyTransformListener().listener() # wait for pose publication self._tf.waitForTransform('bone51', 'base_link', rospy.Time(), rospy.Duration(10.0)) # get neck base translation vector in base_link frame (trans, rot) = self._tf.lookupTransform('base_link', 'bone51', rospy.Time()) self._t5051 = trans # get translation from bone51 to bone52 (trans, rot) = self._tf.lookupTransform('bone51', 'bone52', rospy.Time()) self._t5152 = trans def pointDirectionToHeadPose(self, point_stamped, neck_angle, side_angle, limit_joints=True): ''' Calculate Proto2 head pose so it is oriented toward the given point. :param point_stamped: geometry_msg.msg.PointStamped point to look at. :param neck_angle: joint51 desired angle in radians. :param side_angle: joint54 desired angle in radians. :param limit_joints: limit joints positions to prevent self collisions. :return: sensor_msg.msg.JointState head pose (joint ranges are not checked). Return `None` if transform is not available. ''' joints = JointState() joints.header = Header() joints.header.stamp = rospy.Time.now() # set known fields joints.name = ['joint51', 'joint52', 'joint53', 'joint54'] joints.position = [neck_angle, 0.0, 0.0, side_angle] if limit_joints: joints.position = self.limitHeadJoints51to54(joints.position) neck_angle = joints.position[0] side_angle = joints.position[3] try: # convert point coordinates to base_link frame point_stamped = self._tf.transformPoint( 'base_link', PointStamped( header=Header(frame_id=point_stamped.header.frame_id), point=point_stamped.point)) except tf.Exception as e: Logger.logwarn('Cannot transform to base_link:\n%s' % str(e)) return None # move to neck base direction = [ point_stamped.point.x, point_stamped.point.y, point_stamped.point.z, 1.0 ] upward = [0.0, 0.0, 1.0, 1.0] direction[:3] = numpy.subtract(direction[:3], self._t5051) # rotate to neck_angle around Ox to transform it to desired bone51 frame. direction = numpy.dot(rotation_matrix(neck_angle, (0, 1, 0)), direction) upward = numpy.dot(rotation_matrix(neck_angle, (0, 1, 0)), upward) # move origin to bone52 direction[:3] = numpy.subtract(direction[:3], self._t5152) # calculate rotation angles print(upward) joints.position[1] = math.atan2(direction[1], direction[0]) upward = numpy.dot(rotation_matrix(joints.position[1], (0, 0, 1)), upward) print(upward) joints.position[2] = math.atan2( direction[2], math.sqrt(direction[0]**2 + direction[1]**2)) upward = numpy.dot(rotation_matrix(joints.position[2], (0, 1, 0)), upward) print(upward) joints.position[3] = math.atan2(upward[1], upward[2]) + side_angle # limit joints if limit_joints: joints.position = self.limitHeadJoints51to54(joints.position) return joints def pointDirectionToEyesPose(self, point_stamped): ''' Calculate Proto2 eyes angles for deprecated eyes module. :param point_stamped: geometry_msg.msg.PointStamped point to look at. :return: sensor_msg.msg.JointState head pose (joint ranges are not checked). Return `None` if transform is not available. ''' joints = JointState() joints.header = Header() joints.header.stamp = rospy.Time.now() # set known fields joints.name = ['eyes_pitch', 'eyes_yaw'] joints.position = [0.0, 0.0] try: # convert point coordinates to base_link frame point_stamped = self._tf.transformPoint( 'bone54', PointStamped( header=Header(frame_id=point_stamped.header.frame_id), point=point_stamped.point)) except tf.Exception as e: Logger.logwarn('Cannot transform to bone54:\n%s' % str(e)) return None # calculate angles direction = [ point_stamped.point.x, point_stamped.point.y, point_stamped.point.z ] joints.position[0] = math.atan2( point_stamped.point.z, math.sqrt(point_stamped.point.x**2 + point_stamped.point.y**2)) joints.position[1] = -math.atan2( point_stamped.point.y, math.sqrt(point_stamped.point.x**2 + point_stamped.point.z**2)) # limit joints position joints.position = list( numpy.clip(joints.position, -numpy.pi / 2, numpy.pi / 2)) return joints def limitHeadJoints51to54(self, position): ''' Clip joint51, joint52, joint53, joint54 to collision safe diapazone. :param position: array of joint positions in natural order (joint51,.. joint54) :return: clipped array. ''' return list( numpy.clip(position, [-1.00, -1.57, -0.55, -0.65], [0.25, 1.57, 1.0, 0.65]))
class RandPoseGenerator(EventState): ''' Periodically generate geometry_msgs/PoseStamped messge in given parallelepiped. Frames for parallelepiped is defined and the frame in which message is published can be different. -- topic string Topic where PoseStamped message should be published. -- duration float How long this state will be executed (seconds). -- interval float[2] Array of floats, maximal and minimal interval between movements. -- minXYZ float[3] Max values for x, y, z coordinates. -- maxXYZ float[3] Min values for x, y, z coordinates. -- frame_xyz string Coordinate frame of parallelepiped. -- frame_out string Frame in which pose is publised. <= done Finished. <= failed Failed to activate FollowJointState controller. ''' def __init__(self, topic='motion/controller/look_at/in_pose_ref', duration=120, interval=[3, 5], maxXYZ=[1, 0.3, 0.5], minXYZ=[1.0, -0.3, 0.0], frame_xyz='base_link', frame_out='odom_combined'): super(RandPoseGenerator, self).__init__(outcomes=['done', 'failure']) # Store topic parameter for later use. if not isinstance(topic, str): raise ValueError("Topic name must be string.") if not isinstance(duration, (int, float)) or duration <= 0: raise ValueError("Duration must be positive number.") if len(interval) != 2 or any([ not isinstance(t, (int, float)) for t in interval ]) or any([t < 0 for t in interval]) or interval[0] > interval[1]: raise ValueError("Interval must represent interval of time.") if len(minXYZ) != 3 or any( [not isinstance(v, (int, float)) for v in minXYZ]): raise ValueError("minXYZ: list of three numbers was expected.") if len(maxXYZ) != 3 or any( [not isinstance(v, (int, float)) for v in maxXYZ]): raise ValueError("maxXYZ: list of three numbers was expected.") if not isinstance(frame_xyz, str) or not isinstance(frame_out, str): raise ValueError("Frame names must be string.") self._topic = topic self._duration = Duration.from_sec(duration) self._interval = interval self._minXYZ = minXYZ self._maxXYZ = maxXYZ self._frame_in = frame_xyz self._frame_out = frame_out # create proxies self._publisher = ProxyPublisher({self._topic: PoseStamped}) self._tf_listener = ProxyTransformListener().listener() self._tf_listener.waitForTransform(self._frame_out, self._frame_in, rospy.Time(), rospy.Duration(1)) if not self._tf_listener.canTransform(self._frame_out, self._frame_in, rospy.Time(0)): raise ValueError( "Unable to perform transform between frames %s and %s." % (self._frame_out, self._frame_in)) # state self._start_timestamp = None self._movement_timestamp = None self._movement_duration = Duration() # error in enter hook self._error = False def on_enter(self, userdata): self._error = False # set start timestamp self._start_timestamp = Time.now() self._movement_timestamp = Time.now() Logger.loginfo('Start random pose generation, topic `%s`.' % self._topic) def execute(self, userdata): if self._error: return 'failure' time_now = Time.now() # check if time elasped if time_now - self._start_timestamp > self._duration: return 'done' # check if we have tosend new point if time_now - self._movement_timestamp > self._movement_duration: # determine new interval self._movement_timestamp = time_now self._movement_duration = Duration.from_sec( random.uniform(*self._interval)) # form message msg = PoseStamped() msg.header.frame_id = self._frame_in msg.header.stamp = Time(0.0) # random position msg.pose.position.x = random.uniform(self._minXYZ[0], self._maxXYZ[0]) msg.pose.position.y = random.uniform(self._minXYZ[1], self._maxXYZ[1]) msg.pose.position.z = random.uniform(self._minXYZ[2], self._maxXYZ[2]) # transform to output frame try: # transform self._pose = self._tf_listener.transformPose( self._frame_out, msg) except tf.Exception as e: Logger.logwarn('Unable to transform from %s to %s: %s' % (self._frame_in, self._frame_out, e)) return 'failure' # publish pose msg.header.stamp = self._movement_timestamp self._publisher.publish(self._topic, msg) def on_exit(self, userdata): Logger.loginfo('Done random pose generation.')
class DetectPipesSM(Behavior): ''' Position as required and try different ways to detect the pipe positions ''' def __init__(self): super(DetectPipesSM, self).__init__() self.name = 'Detect Pipes' # parameters of this behavior self.add_parameter('pose_offset', 0.3) self.add_parameter('close_offset', 0.1) # references to used behaviors # Additional initialization code can be added inside the following tags # [MANUAL_INIT] self._tf = ProxyTransformListener().listener() # [/MANUAL_INIT] # Behavior comments: # O 98 480 # Run in full autonomy to skip looking, run in high to decide def create(self): joints_arm_with_gripper = ['arm_joint_%d'%i for i in range(5)] srdf = "hector_tracker_robot_moveit_config/config/taurob_tracker.srdf" percept_class = 'pipes' move_group = "arm_with_gripper_group" move_group_no_eef = "arm_group" # x:268 y:162, x:472 y:46 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.object_pose = None _state_machine.userdata.object_type = percept_class _state_machine.userdata.test_pregrasp = [0, 1.57, 0.77, 0.38, 0] _state_machine.userdata.index = -1 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365, x:569 y:352, x:45 y:183 _sm_gripper_action_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'back']) with _sm_gripper_action_0: # x:168 y:53 OperatableStateMachine.add('Decide_Close_Gripper', OperatorDecisionState(outcomes=['close_gripper', 'move_back'], hint="Grasp pipe?", suggestion='close_gripper'), transitions={'close_gripper': 'Close_Gripper', 'move_back': 'back'}, autonomy={'close_gripper': Autonomy.Full, 'move_back': Autonomy.Full}) # x:221 y:173 OperatableStateMachine.add('Close_Gripper', GripperState(action=0.8, duration=2.0), transitions={'done': 'Manipulate_Pipe', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.High}) # x:229 y:391 OperatableStateMachine.add('Decide_Open_Gripper', OperatorDecisionState(outcomes=['open_gripper', 'move_back'], hint="Open gripper or pull pipe?", suggestion='open_gripper'), transitions={'open_gripper': 'Open_Gripper', 'move_back': 'back'}, autonomy={'open_gripper': Autonomy.High, 'move_back': Autonomy.Full}) # x:241 y:290 OperatableStateMachine.add('Manipulate_Pipe', LogState(text="Perform desired pipe manipulation", severity=Logger.REPORT_HINT), transitions={'done': 'Decide_Open_Gripper'}, autonomy={'done': Autonomy.High}) # x:215 y:511 OperatableStateMachine.add('Open_Gripper', GripperState(action=GripperState.OPEN, duration=1.0), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.High}) # x:153 y:384, x:388 y:221 _sm_move_back_out_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['pipes_out_pose']) with _sm_move_back_out_1: # x:118 y:87 OperatableStateMachine.add('Get_Cartesian_Path_Back', CalculateCartesianPathState(move_group=move_group, ignore_collisions=True), transitions={'planned': 'Execute_Cartesian_Path_Back', 'failed': 'failed'}, autonomy={'planned': Autonomy.Low, 'failed': Autonomy.High}, remapping={'eef_pose': 'pipes_out_pose', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:116 y:216 OperatableStateMachine.add('Execute_Cartesian_Path_Back', MoveitExecuteTrajectoryState(), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:230 y:517, x:550 y:229 _sm_move_to_close_pose_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['pipes_pose']) with _sm_move_to_close_pose_2: # x:97 y:43 OperatableStateMachine.add('Move_Pose_Cartesian', CalculationState(calculation=lambda p: self.update_pose(p, self.close_offset)), transitions={'done': 'Visualize_Pipes_Close_Pose'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pipes_pose', 'output_value': 'pipes_close_pose'}) # x:113 y:140 OperatableStateMachine.add('Visualize_Pipes_Close_Pose', PublishPoseState(topic='/pipes/close_pose'), transitions={'done': 'Get_Cartesian_Path'}, autonomy={'done': Autonomy.Low}, remapping={'pose': 'pipes_close_pose'}) # x:147 y:371 OperatableStateMachine.add('Execute_Cartesian_Path', MoveitExecuteTrajectoryState(), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:121 y:260 OperatableStateMachine.add('Get_Cartesian_Path', CalculateCartesianPathState(move_group=move_group, ignore_collisions=True), transitions={'planned': 'Execute_Cartesian_Path', 'failed': 'failed'}, autonomy={'planned': Autonomy.Low, 'failed': Autonomy.High}, remapping={'eef_pose': 'pipes_close_pose', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:119 y:516, x:358 y:275 _sm_move_to_out_pose_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['pipes_pose'], output_keys=['pipes_out_pose']) with _sm_move_to_out_pose_3: # x:83 y:35 OperatableStateMachine.add('Move_Pose_Out', CalculationState(calculation=lambda p: self.update_pose(p, self.pose_offset)), transitions={'done': 'Visualize_Pipes_Out_Pose'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pipes_pose', 'output_value': 'pipes_out_pose'}) # x:85 y:307 OperatableStateMachine.add('Move_To_Joints', MoveitToJointsState(move_group=move_group, joint_names=joints_arm_with_gripper, action_topic='/move_group'), transitions={'reached': 'Correct_Gripper_Rotation', 'planning_failed': 'failed', 'control_failed': 'failed'}, autonomy={'reached': Autonomy.Low, 'planning_failed': Autonomy.High, 'control_failed': Autonomy.High}, remapping={'joint_config': 'joint_config'}) # x:77 y:118 OperatableStateMachine.add('Visualize_Pipes_Out_Pose', PublishPoseState(topic='/pipes/out_pose'), transitions={'done': 'Get_Joint_Target'}, autonomy={'done': Autonomy.Low}, remapping={'pose': 'pipes_out_pose'}) # x:83 y:211 OperatableStateMachine.add('Get_Joint_Target', CalculateIKState(move_group=move_group, ignore_collisions=False), transitions={'planned': 'Move_To_Joints', 'failed': 'failed'}, autonomy={'planned': Autonomy.Low, 'failed': Autonomy.High}, remapping={'eef_pose': 'pipes_out_pose', 'joint_config': 'joint_config'}) # x:71 y:408 OperatableStateMachine.add('Correct_Gripper_Rotation', GripperRollState(rotation=GripperRollState.HORIZONTAL, duration=1.0), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.High}) # x:272 y:264, x:55 y:574 _sm_eef_to_detected_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['pipes_pose'], output_keys=['pipes_pose']) with _sm_eef_to_detected_4: # x:58 y:35 OperatableStateMachine.add('Move_To_Out_Pose', _sm_move_to_out_pose_3, transitions={'finished': 'Decide_Move_Close', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pipes_pose': 'pipes_pose', 'pipes_out_pose': 'pipes_out_pose'}) # x:568 y:38 OperatableStateMachine.add('Move_To_Close_Pose', _sm_move_to_close_pose_2, transitions={'finished': 'Gripper_Action', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pipes_pose': 'pipes_pose'}) # x:175 y:448 OperatableStateMachine.add('Move_Back_Out', _sm_move_back_out_1, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pipes_out_pose': 'pipes_out_pose'}) # x:326 y:34 OperatableStateMachine.add('Decide_Move_Close', OperatorDecisionState(outcomes=['close', 'end'], hint="Move closer to pipe?", suggestion='close'), transitions={'close': 'Move_To_Close_Pose', 'end': 'finished'}, autonomy={'close': Autonomy.High, 'end': Autonomy.Full}) # x:626 y:219 OperatableStateMachine.add('Gripper_Action', _sm_gripper_action_0, transitions={'finished': 'Decide_Move_Back', 'failed': 'failed', 'back': 'Move_Back_Out'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'back': Autonomy.Inherit}) # x:408 y:452 OperatableStateMachine.add('Decide_Move_Back', OperatorDecisionState(outcomes=['move_back', 'end'], hint="Move back from pipe?", suggestion='move_back'), transitions={'move_back': 'Move_Back_Out', 'end': 'finished'}, autonomy={'move_back': Autonomy.High, 'end': Autonomy.Full}) # x:30 y:365, x:130 y:365 _sm_select_pipe_5 = OperatableStateMachine(outcomes=['finished', 'next'], input_keys=['index', 'object_pose', 'object_data'], output_keys=['index', 'pipe_pose']) with _sm_select_pipe_5: # x:98 y:78 OperatableStateMachine.add('Increment_Index', CalculationState(calculation=lambda x: x+1), transitions={'done': 'Select_Pipe'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'index', 'output_value': 'index'}) # x:102 y:178 OperatableStateMachine.add('Select_Pipe', SelectPipePose(), transitions={'selected': 'next', 'out_of_range': 'finished'}, autonomy={'selected': Autonomy.Off, 'out_of_range': Autonomy.Off}, remapping={'object_pose': 'object_pose', 'object_data': 'object_data', 'pose_choice': 'index', 'pipe_pose': 'pipe_pose'}) # x:427 y:433, x:453 y:131 _sm_get_pipes_6 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['object_pose'], output_keys=['object_pose', 'object_data']) with _sm_get_pipes_6: # x:51 y:28 OperatableStateMachine.add('Get_Observation_Arm_Config', GetJointsFromSrdfState(config_name="observation_pose", srdf_file=srdf, move_group="", robot_name=""), transitions={'retrieved': 'Move_To_Observation_Pose', 'file_error': 'failed'}, autonomy={'retrieved': Autonomy.Off, 'file_error': Autonomy.Off}, remapping={'joint_values': 'joints_observation_pose'}) # x:53 y:111 OperatableStateMachine.add('Move_To_Observation_Pose', MoveitToJointsState(move_group="arm_group", joint_names=joints_arm_with_gripper[0:4], action_topic='/move_group'), transitions={'reached': 'Detect_Pipes', 'planning_failed': 'failed', 'control_failed': 'failed'}, autonomy={'reached': Autonomy.Low, 'planning_failed': Autonomy.High, 'control_failed': Autonomy.High}, remapping={'joint_config': 'joints_observation_pose'}) # x:67 y:303 OperatableStateMachine.add('Get_Pipes_Pose', MonitorPerceptState(required_support=0, percept_class='pipes', track_percepts=False), transitions={'detected': 'Visualize_Pipes_Pose'}, autonomy={'detected': Autonomy.Off}, remapping={'object_id': 'object_id', 'object_pose': 'object_pose', 'object_data': 'object_data'}) # x:59 y:396 OperatableStateMachine.add('Visualize_Pipes_Pose', PublishPoseState(topic='/pipes/center_pose'), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Low}, remapping={'pose': 'object_pose'}) # x:72 y:210 OperatableStateMachine.add('Detect_Pipes', PipeDetectionState(max_attempts=100), transitions={'found': 'Get_Pipes_Pose', 'unknown': 'failed'}, autonomy={'found': Autonomy.Low, 'unknown': Autonomy.High}) with _state_machine: # x:94 y:72 OperatableStateMachine.add('Get_Pipes', _sm_get_pipes_6, transitions={'finished': 'Select_Pipe', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'object_pose': 'object_pose', 'object_data': 'object_data'}) # x:492 y:328 OperatableStateMachine.add('Move_To_Pregrasp', MoveitToJointsState(move_group="arm_group", joint_names=joints_arm_with_gripper[0:4], action_topic='/move_group'), transitions={'reached': 'EEF_To_Detected', 'planning_failed': 'failed', 'control_failed': 'failed'}, autonomy={'reached': Autonomy.Low, 'planning_failed': Autonomy.High, 'control_failed': Autonomy.High}, remapping={'joint_config': 'test_pregrasp'}) # x:87 y:328 OperatableStateMachine.add('Decide_Look_At', OperatorDecisionState(outcomes=['focus', 'skip'], hint="Should sensor head focus pipes?", suggestion='skip'), transitions={'focus': 'Look_At_Target', 'skip': 'Move_To_Pregrasp'}, autonomy={'focus': Autonomy.Full, 'skip': Autonomy.High}) # x:300 y:428 OperatableStateMachine.add('Look_At_Target', LookAtWaypoint(), transitions={'reached': 'Move_To_Pregrasp', 'failed': 'failed'}, autonomy={'reached': Autonomy.High, 'failed': Autonomy.Full}, remapping={'waypoint': 'pipes_pose'}) # x:94 y:222 OperatableStateMachine.add('Select_Pipe', _sm_select_pipe_5, transitions={'finished': 'finished', 'next': 'Decide_Look_At'}, autonomy={'finished': Autonomy.Inherit, 'next': Autonomy.Inherit}, remapping={'index': 'index', 'object_pose': 'object_pose', 'object_data': 'object_data', 'pipe_pose': 'pipes_pose'}) # x:783 y:322 OperatableStateMachine.add('EEF_To_Detected', _sm_eef_to_detected_4, transitions={'finished': 'Select_Pipe', 'failed': 'Select_Pipe'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pipes_pose': 'pipes_pose'}) return _state_machine # Private functions can be added inside the following tags # [MANUAL_FUNC] def update_pose(self, pose, offset): new_pose = self._tf.transformPose('base_link', pose) new_pose.pose.position.x -= offset new_pose.pose.orientation.x = 0 new_pose.pose.orientation.y = 0 new_pose.pose.orientation.z = 0 new_pose.pose.orientation.w = 1 return new_pose
class BrohoofSM(Behavior): ''' Move leg1 up to specified target, perform greeting, wait and pt leg down. Robot is assumed to be standing on four legs. ''' def __init__(self): super(BrohoofSM, self).__init__() self.name = 'Brohoof' # parameters of this behavior self.add_parameter('wait_time', 5) self.add_parameter('neck_angle', 0.25) self.add_parameter('brohoof_cone', 0.78) self.add_parameter('brohoof_upper_limit', 0.30) # references to used behaviors # Additional initialization code can be added inside the following tags # [MANUAL_INIT] self._tf = ProxyTransformListener().listener() # [/MANUAL_INIT] # Behavior comments: def create(self): # x:228 y:396, x:225 y:240, x:1010 y:659 _state_machine = OperatableStateMachine(outcomes=['finished', 'unreachable', 'failed'], input_keys=['pose']) _state_machine.userdata.pose = PoseStamped(Header(frame_id = 'base_link'), Pose(Point(0.4, 0.0, -0.05), Quaternion(0, 0, 0, 1))) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:118 y:85 OperatableStateMachine.add('CheckHandPosition', DecisionState(outcomes=['good','bad'], conditions=self.checkHandPosition), transitions={'good': 'GetHeadPose', 'bad': 'unreachable'}, autonomy={'good': Autonomy.Off, 'bad': Autonomy.Off}, remapping={'input_value': 'pose'}) # x:890 y:253 OperatableStateMachine.add('SayHello', TextCommandState(type='voice/play_wav', command='hello_im_sweetie_bot_friedship_programms', topic='control'), transitions={'done': 'Wait'}, autonomy={'done': Autonomy.Off}) # x:663 y:69 OperatableStateMachine.add('RaiseHead', MoveitToJointsState(move_group='head', joint_names=['joint51', 'joint52', 'joint53', 'joint54'], action_topic='move_group'), transitions={'reached': 'RaiseLegProgram', 'planning_failed': 'failed', 'control_failed': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off}, remapping={'joint_config': 'head_pose'}) # x:309 y:66 OperatableStateMachine.add('GetHeadPose', GetJointValuesState(joints=['joint51', 'joint52', 'joint53', 'joint54']), transitions={'retrieved': 'CalcRaisedHeadPose'}, autonomy={'retrieved': Autonomy.Off}, remapping={'joint_values': 'head_pose'}) # x:485 y:73 OperatableStateMachine.add('CalcRaisedHeadPose', CalculationState(calculation=self.calculateHeadPoseFunc), transitions={'done': 'RaiseHead'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'head_pose', 'output_value': 'head_pose'}) # x:891 y:65 OperatableStateMachine.add('RaiseLegProgram', ExecuteJointTrajectory(action_topic='motion/controller/joint_trajectory', trajectory_param='brohoof_begin', trajectory_ns='/saved_msgs/joint_trajectory'), transitions={'success': 'SayHello', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed'}, autonomy={'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off}, remapping={'result': 'result'}) # x:510 y:396 OperatableStateMachine.add('ReturnLegProgrammed', ExecuteJointTrajectory(action_topic='motion/controller/joint_trajectory', trajectory_param='brohoof_end', trajectory_ns='/saved_msgs/joint_trajectory'), transitions={'success': 'finished', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed'}, autonomy={'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off}, remapping={'result': 'result'}) # x:851 y:399 OperatableStateMachine.add('Wait', WaitState(wait_time=self.wait_time), transitions={'done': 'ReturnLegProgrammed'}, autonomy={'done': Autonomy.Off}) return _state_machine # Private functions can be added inside the following tags # [MANUAL_FUNC] def calculateHeadPoseFunc(self, joints): joints = list(joints) joints[0] = 0.0 joints[2] = self.neck_angle return joints def checkHandPosition(self, input_pose): ''' Check if hand is in good position for brohoof. Return 'good' or 'bad'. ''' # check if frame header presents if not input_pose.header.frame_id: Logger.logerr('checkHandPosition: Header is missing.') return 'bad' # convert ot base_link frame try: output_pose = self._tf.transformPose('base_link', PoseStamped(Header(frame_id = input_pose.header.frame_id), input_pose.pose)) except tf.Exception as e: Logger.logerr('calculateHeadPoseFunc: Cannot transform from `%s` to `base_link` frame.' % input_pose.header.frame_id) return 'bad' pos = output_pose.pose.position Logger.loginfo('calculateHeadPoseFunc: object pos: %s' % str(pos)) # Check if boundarie are good if pos.z > self.brohoof_upper_limit or pos.z < -0.15: # too high or to low return 'bad' if pos.x < 0.25: # too close return 'bad' # Move to shoulder1 point pos.x -= 0.080 pos.y -= 0.037 pos.z += 0.027 # Check if angle is out of cone if abs(math.atan2(pos.y,pos.x)) > self.brohoof_cone: # out of cone return 'bad' return 'good'
class CreatePurePursuitState(EventState): ''' This state calculates the twist required to follow a constant curvature arc to the pure pursuit intersection point. The command is published as a TwistStamped command based on parameters. The state monitors the iRobot Create bumper status, and stops and returns a failed outcome if a bumper is activated. If arc motion is used, the arc should be less than or equal to pi/2 radians. Use multiple targets for longer arcs. .......-- desired_velocity float Desired velocity in m/s (default: 0.2) .......-- max_rotation_rate float Maximum rotation rate radians/s (default: 10.0) -- target_frame: string Coordinate frame of target point (default: 'map') -- target_x: float Target point x-coordinate (m) -- target_y: float Target point y-coordinate (m) -- target_type: string Desired motion ('line','arc') (default: 'line') -- lookahead_distance: float Lookahead distance (m) (default: 0.25) -- timeout float Transform timeout (seconds) (default: 0.08) -- recover_mode bool Recover path (typically on startup) (default: False) -- center_x: float Center point x-coordinate for circle defining arc motion (default: 0.0) -- center_y: float Center point y-coordinate for circle defining arc motion (default: 0.0) -- cmd_topic string topic name of the robot command (default: '/create_node/cmd_vel') -- sensor_topic string topic of the iRobot Create sensor state (default: '/create_node/sensor_state') -- odometry_topic: string topic of the iRobot Create sensor state (default: '/create_node/odom' -- marker_topic: string topic of the RViz marker used for visualization (default: '/pure_pursuit_marker') -- marker_size: float Size of RViz marker used for target (default: 0.05) ># prior_point object Prior waypoint at start of this segment #> target_point object Target point at end of this calculation (becomes next node's prior) <= done Reached the end of target relevance <= failed A bumper was activated prior to completion ''' def __init__(self, desired_velocity=0.2, max_rotation_rate=10.0, target_frame='map', target_x=1.0, target_y=0.1, target_type='line', lookahead_distance=0.25, timeout=0.08, recover_mode=False, center_x=0.0, center_y=0.0, cmd_topic='/create_node/cmd_vel', sensor_topic='/create_node/sensor_state', odometry_topic='/create_node/odom', marker_topic='/pure_pursuit_marker', marker_size=0.05): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(CreatePurePursuitState, self).__init__(outcomes=['done', 'failed'], input_keys=['prior_point'], output_keys=['target_point']) # Store state parameter for later use. self._twist = TwistStamped() self._twist.twist.linear.x = desired_velocity self._twist.twist.angular.z = 0.0 self._desired_velocity = desired_velocity self._max_rotation_rate = max_rotation_rate # Used to clamp the rotation calculations self._current_position = PointStamped() self._current_position.header.stamp = rospy.Time.now() self._current_position.header.frame_id = target_frame self._target = PointStamped() self._target.header.stamp = rospy.Time.now() self._target.header.frame_id = target_frame self._target.point = Point(target_x, target_y, 0.0) self._center = PointStamped() self._center.header = self._target.header self._center.point = Point(center_x, center_y, 0.0) self._lookahead = lookahead_distance self._recover_mode = recover_mode self._target_type = target_type self._target_frame = target_frame if (self._target_type == 'arc'): self._radius = np.sqrt((center_x - target_x)**2 + (center_y - target_y)**2) Logger.loginfo( 'Using arc type with center=(%d, %d) target=(%d,%d) radius=%d' % (self._center.point.x, self._center.point.y, self._target.point.x, self._target.point.y, self._radius)) self._odom_frame = 'odom' # Update with the first odometry message self._robot_frame = 'base_footprint' self._failed = False self._timeout = rospy.Duration(timeout) # transform timeout # The constructor is called when building the state machine, not when actually starting the behavior. # Thus, we cannot save the starting time now and will do so later. self._start_time = None self._done = None # Track the outcome so we can detect if transition is blocked self._odom_topic = odometry_topic self._sensor_topic = sensor_topic self._marker_topic = marker_topic self._cmd_topic = cmd_topic self._listener = ProxyTransformListener() self._sensor_sub = ProxySubscriberCached( {self._sensor_topic: CreateSensorState}) self._odom_sub = ProxySubscriberCached({self._odom_topic: Odometry}) self._pub = ProxyPublisher({self._cmd_topic: TwistStamped}) if (self._marker_topic != ""): self._marker_pub = ProxyPublisher({self._marker_topic: Marker}) else: self._marker_pub = None self._marker = Marker() self._marker.header.frame_id = self._target_frame self._marker.header.stamp = rospy.Time.now() self._marker.ns = "pure_pursuit_waypoints" self._marker.id = int(target_x * 1000000) + int(target_y * 1000) self._marker.type = Marker.SPHERE self._marker.action = Marker.ADD self._marker.pose.position.x = target_x self._marker.pose.position.y = target_y self._marker.pose.position.z = 0.0 self._marker.pose.orientation.x = 0.0 self._marker.pose.orientation.y = 0.0 self._marker.pose.orientation.z = 0.0 self._marker.pose.orientation.w = 1.0 self._marker.scale.x = marker_size self._marker.scale.y = marker_size self._marker.scale.z = marker_size self._marker.color.a = 1.0 # Don't forget to set the alpha! self._marker.color.r = 0.0 self._marker.color.g = 0.0 self._marker.color.b = 1.0 self._reference_marker = Marker() self._reference_marker.header.frame_id = self._target_frame self._reference_marker.header.stamp = rospy.Time.now() self._reference_marker.ns = "pure_pursuit_reference" self._reference_marker.id = 1 self._reference_marker.type = Marker.SPHERE self._reference_marker.action = Marker.ADD self._reference_marker.pose.position.x = target_x self._reference_marker.pose.position.y = target_y self._reference_marker.pose.position.z = 0.0 self._reference_marker.pose.orientation.x = 0.0 self._reference_marker.pose.orientation.y = 0.0 self._reference_marker.pose.orientation.z = 0.0 self._reference_marker.pose.orientation.w = 1.0 self._reference_marker.scale.x = marker_size * 0.75 self._reference_marker.scale.y = marker_size * 0.75 self._reference_marker.scale.z = marker_size * 0.75 self._reference_marker.color.a = 0.0 # Add, but make invisible at first self._reference_marker.color.r = 1.0 self._reference_marker.color.g = 0.0 self._reference_marker.color.b = 1.0 self._local_target_marker = Marker() self._local_target_marker.header.frame_id = self._target_frame self._local_target_marker.header.stamp = rospy.Time.now() self._local_target_marker.ns = "pure_pursuit_target" self._local_target_marker.id = 1 self._local_target_marker.type = Marker.SPHERE self._local_target_marker.action = Marker.ADD self._local_target_marker.pose.position.x = target_x self._local_target_marker.pose.position.y = target_y self._local_target_marker.pose.position.z = 0.0 self._local_target_marker.pose.orientation.x = 0.0 self._local_target_marker.pose.orientation.y = 0.0 self._local_target_marker.pose.orientation.z = 0.0 self._local_target_marker.pose.orientation.w = 1.0 self._local_target_marker.scale.x = marker_size self._local_target_marker.scale.y = marker_size self._local_target_marker.scale.z = marker_size self._local_target_marker.color.a = 0.0 # Add, but make invisible at first self._local_target_marker.color.r = 1.0 self._local_target_marker.color.g = 0.0 self._local_target_marker.color.b = 1.0 # Transform point into odometry frame def transformOdom(self, point): try: # Get transform self._listener.listener().waitForTransform(self._odom_frame, point.header.frame_id, point.header.stamp, self._timeout) return self._listener.listener().transformPoint( self._odom_frame, point) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: Logger.logwarn('Failed to get the transformation\n %s' % str(e)) self._failed = True return None except Exception as e: Logger.logwarn( 'Failed to get the transformation due to unknown error\n %s' % str(e)) self._failed = True return None # Transform point into map frame def transformMap(self, odometry): odom_position = PointStamped() odom_position.header = odometry.header odom_position.point = odometry.pose.pose.position try: # Get transform self._listener.listener().waitForTransform( self._target_frame, odometry.header.frame_id, odometry.header.stamp, self._timeout) return self._listener.listener().transformPoint( self._target_frame, odom_position) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: Logger.logwarn( 'Failed to get the transformation to target_frame\n %s' % str(e)) self._failed = True return None except Exception as e: Logger.logwarn( 'Failed to get the transformation to target frame due to unknown error\n %s' % str(e)) self._failed = True return None # Transform point into robot body frame def transformBody(self, point): try: # Get transform self._listener.listener().waitForTransform(self._robot_frame, point.header.frame_id, point.header.stamp, self._timeout) return self._listener.listener().transformPoint( self._robot_frame, point) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: Logger.logwarn('Failed to get the transformation\n %s' % str(e)) self._failed = True return None except: Logger.logwarn( 'Failed to get the transformation due to unknown error\n') self._failed = True return None def execute(self, userdata): # This method is called periodically while the state is active. # If no outcome is returned, the state will stay active. if (self._done): # We have completed the state, and therefore must be blocked by autonomy level # Stop the robot, but and return the prior outcome ts = TwistStamped() ts.header.stamp = rospy.Time.now() self._pub.publish(self._cmd_topic, ts) userdata.target_point = self._target # Set the user data for passing to next node return self._done # Check bumpers to see if we hit something if (self._sensor_sub.has_msg(self._sensor_topic)): # check the status of the bumpers sensors = self._sub.get_last_msg(self._sensor_topic) if ((sensors.bumps_wheeldrops > 0) or sensors.cliff_left or sensors.cliff_front_left or sensors.cliff_front_right or sensors.cliff_right): ts = TwistStamped() ts.header.stamp = rospy.Time.now() self._pub.publish(self._cmd_topic, ts) userdata.target_point = self._target # Set the user data for passing to next node Logger.logwarn( '%s Bumper contact = %d cliff: left=%d %d %d %d = right ' % (self.name, sensors.bumps_wheeldrops, sensors.cliff_left, sensors.cliff_front_left, sensors.cliff_front_right, sensors.cliff_right)) self._done = 'failed' return 'failed' # Get the latest odometry data if (self._sub.has_msg(self._odom_topic)): self._last_odom = self._sub.get_last_msg(self._odom_topic) # Update the current pose self._current_position = self.transformMap(self._last_odom) if (self._failed): userdata.target_point = self._target # Set the user data for passing to next node self._done = 'failed' return 'failed' # Transform the target points into the current odometry frame self._target.header.stamp = self._last_odom.header.stamp local_target = self._target #self.transformOdom(self._target) # If target point is withing lookahead distance then we are done dr = np.sqrt( (local_target.point.x - self._current_position.point.x)**2 + (local_target.point.y - self._current_position.point.y)**2) if (dr < self._lookahead): Logger.loginfo( ' %s Lookahead circle is past target - done! target=(%f, %f, %f) robot=(%f,%f, %f) dr=%f lookahead=%f ' % (self.name, local_target.point.x, local_target.point.y, local_target.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, dr, self._lookahead)) userdata.target_point = self._target # Set the user data for passing to next node self._done = 'done' return 'done' # Transform the prior target point into the current odometry frame self._prior_point.header.stamp = self._last_odom.header.stamp local_prior = self._prior_point #self.transformOdom(self._prior_point) if (self._failed): userdata.target_point = self._target # Set the user data for passing to next node self._done = 'failed' return 'failed' # Assume we can go the desired velocity self._twist.twist.linear.x = self._desired_velocity lookahead = None if (self._target_type == 'arc'): lookahead = self.calculateArcTwist(local_prior, local_target) else: lookahead = self.calculateLineTwist(local_prior, local_target) if (lookahead is None): # Did not get a lookahead point so we either failed, completed segment, or are deliberately holding the prior velocity (to recover from minor perturbations) if (self._done is not None): userdata.target_point = self._target # Set the user data for passing to next node return self._done # return what was set (either 'failed' or 'done') # Sanity check the rotation rate if (math.fabs(self._twist.twist.angular.z) > self._max_rotation_rate): self._twist.twist.linear.x = self._desired_velocity * self._max_rotation_rate / math.fabs( self._twist.twist.angular.z) # decrease the speed self._twist.twist.angular.z = math.copysign( self._max_rotation_rate, self._twist.twist.angular.z) # Normal operation - publish the latest calculated twist self._twist.header.stamp = rospy.Time.now() self._pub.publish(self._cmd_topic, self._twist) if (self._marker_pub): self._marker_pub.publish(self._marker_topic, self._reference_marker) self._marker_pub.publish(self._marker_topic, self._local_target_marker) return None def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. self._start_time = rospy.Time.now() self._done = None # reset the completion flag self._failed = False # reset the failed flag self._prior_point = userdata.prior_point if (self._marker_pub): self._marker.action = Marker.MODIFY self._marker.color.a = 1.0 self._marker.color.r = 0.0 self._marker.color.g = 1.0 # Indicate this target is active self._marker.color.b = 0.0 self._marker_pub.publish(self._marker_topic, self._marker) #Logger.logdebug(" Moving toward target=%f,%f from position=%f,%f" % # (self._target.point.x, self._target.point.y, # self._current_position.point.x, self._current_position.point.y)) def on_exit(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. elapsed_time = rospy.Time.now() - self._start_time userdata.target_point = self._target # Set the user data for passing to next node #Logger.logdebug(" Spent %f seconds in this segment target=%f,%f position=%f,%f" % # (elapsed_time.to_sec(),self._target.point.x, self._target.point.y, # self._current_position.point.x, self._current_position.point.y)) if (self._marker_pub): self._marker.color.a = 1.0 # Don't forget to set the alpha! self._marker.color.r = 0.8 # Indicate this target is no longer active self._marker.color.g = 0.0 self._marker.color.b = 0.0 self._marker_pub.publish(self._marker_topic, self._marker) def on_start(self): # Wait for odometry message while (not self._odom_sub.has_msg(self._sensor_topic)): Logger.logwarn('Waiting for odometry message from the robot ') rospy.sleep(0.05) while (not self._sub.has_msg(self._odom_topic)): Logger.logwarn( 'Waiting for odometry message to become available from the robot ' ) rospy.sleep(0.25) self._last_odom = self._sub.get_last_msg(self._odom_topic) self._odom_frame = self._last_odom.header.frame_id #Logger.loginfo(' odometry frame id <%s>' % (self._odom_frame)) # Update the target transformation self._target.header.stamp = self._last_odom.header.stamp while (self.transformOdom(self._target) is None): Logger.logwarn( 'Waiting for tf transformations to odometry frame to become available from the robot ' ) rospy.sleep(0.25) self._target.header.stamp = rospy.Time.now() while (self.transformMap(self._last_odom) is None): Logger.logwarn( 'Waiting for tf transformations to map frame become available from the robot ' ) rospy.sleep(0.25) self._last_odom = self._sub.get_last_msg(self._odom_topic) self._current_position = self.transformMap(self._last_odom) # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. if (self._marker_pub): self._marker.action = Marker.ADD self._marker.color.a = 1.0 # Set alpha otherwise the marker is invisible self._marker.color.r = 0.0 self._marker.color.g = 0.0 self._marker.color.b = 1.0 # Indicate this target is planned self._marker_pub.publish(self._marker_topic, self._marker) self._marker_pub.publish(self._marker_topic, self._reference_marker) self._marker_pub.publish(self._marker_topic, self._local_target_marker) self._marker.action = Marker.MODIFY self._reference_marker.action = Marker.MODIFY self._reference_marker.color.a = 1.0 # Set alpha so it will become visible on next publish self._local_target_marker.action = Marker.MODIFY self._local_target_marker.color.a = 1.0 # Set alpha so it will become visible on next publish # Attempt to recover the path for large error def recoverPath(self, local_target, local_prior, pv, qv): pp = pv.x * pv.x + pv.y * pv.y qq = qv.x * qv.x + qv.y * qv.y qp = -qv.x * pv.x - qv.y * pv.y # negate qv dp = qp / pp # fractional distance along the pv vector # Assume we steer toward the initial point control = PointStamped() control.header = local_prior.header control.point = deepcopy(local_prior.point) if (dp > 1.0): # Steer toward the target point control = local_target elif (dp > 0.0): # Steer toward the closest point control.point.x = control.point.x + dp * pv.x # Control point in the odometry frame control.point.y = control.point.y + dp * pv.y # Control point in the odometry frame control.point.z = control.point.z + dp * pv.z # Control point in the odometry frame self._reference_marker.pose.position = deepcopy(control.point) self._local_target_marker.pose.position = deepcopy(local_target.point) control_robot = self.transformBody(control) if (control_robot.point.x <= 0.001): control_robot = self.transformBody(local_target) # One last try if (control_robot.point.x <= 0.001): dist = control_robot.point.x * control_robot.point.x + control_robot.point.y * control_robot.point.y if (dist > 2.5): Logger.loginfo( "recovery control point is behind the robot and far way abort recovery!" ) return False else: # Target is close enough - do a zero radius turn toward the target line until our closet point is ahead self._twist.twist.linear.x = 0.0 self._twist.twist.angular.z = math.copysign( self._desired_velocity / 0.13, control_robot.point.y) return True else: # Target is ahead - do a zero radius turn toward the target line until our closet point is ahead self._twist.twist.linear.x = 0.0 self._twist.twist.angular.z = math.copysign( self._desired_velocity / 0.13, control_robot.point.y) return True # Assume lookahead tangent to the control point dc = Vector3(control.point.x - self._current_position.point.x, control.point.y - self._current_position.point.y, 0.0) curvature = 2.0 * control_robot.point.y / (dc.x * dc.x + dc.y * dc.y) if (np.isnan(curvature)): Logger.logerr("invalid curvature calculation abort recovery!") return False self._twist.twist.angular.z = curvature * self._desired_velocity return True # Method to calculate the lookahead point given line segment from prior to target def calculateLineTwist(self, local_prior, local_target): # Define a line segment from prior to the target (assume 2D) pv = Vector3(local_target.point.x - local_prior.point.x, local_target.point.y - local_prior.point.y, 0.0) qv = Vector3(local_prior.point.x - self._current_position.point.x, local_prior.point.y - self._current_position.point.y, 0.0) # Find intersection of line segment with lookahead circle centered at the robot a = pv.x * pv.x + pv.y * pv.y # b = 2.0 * (qv.x * pv.x + qv.y * pv.y) c = (qv.x * qv.x + qv.y * qv.y) - self._lookahead * self._lookahead if (a < 0.001): Logger.logerr( ' %s Invalid prior and target for line target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f ' % (self.name, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c)) self._done = 'failed' return None discrim = b * b - 4 * a * c if (discrim < 0.0): # No intersection - this should not be unless bad start or localization perturbation if (self._recover_mode): # Attempt to regain path if (not self.recoverPath(local_target, local_prior, pv, qv)): Logger.logwarn( ' %s Path recovery failed for line target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._done = 'failed' return None else: Logger.logwarn( ' %s No path recovery - no intersection for line target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._done = 'failed' return None else: # solve quadratic equation for intersection points sqd = math.sqrt(discrim) t1 = (-b - sqd) / (2 * a) # min value t2 = (-b + sqd) / (2 * a) # max value if (t2 < t1): Logger.logwarn( ' %s Say what!: t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._done = 'failed' return None if (t2 < 0.0): # all intersections are behind the segment - this should not be unless bad start or localization perturbation if (self._recover_mode): # Attempt to regain path if (not self.recoverPath(local_target, local_prior, pv, qv)): Logger.logwarn( ' %s Path recovery failed with circle before segment target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._done = 'failed' return None elif (t2 > -0.1): # likely due to localization perturbation Logger.logwarn( ' %s Circle is before segment - continue prior motion! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) return None else: Logger.logwarn( ' %s Circle is before segment! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._done = 'failed' return None elif (t1 > 1.0): # all intersections are past the segment Logger.loginfo( ' %s Circle is past segment - done! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._done = 'done' return None elif (t1 < 0.0 and t2 > 1.0): # Segment is contained inside the lookahead circle Logger.loginfo( ' %s Circle contains segment - move along!: t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._done = 'done' return None elif (t2 > 1.0): # The lookahead circle extends beyond the target point - we are finished here Logger.loginfo( ' %s Circle extends past segment - done! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._done = 'done' return None else: # This is the normal case # Must be line segment control = deepcopy(local_prior) control.point.x = control.point.x + t2 * pv.x # Control point in the odometry frame control.point.y = control.point.y + t2 * pv.y # Control point in the odometry frame control.point.z = control.point.z + t2 * pv.z # Control point in the odometry frame self._reference_marker.pose.position = control.point self._local_target_marker.pose.position = local_target.point control_robot = self.transformBody(control) curvature = 2.0 * control_robot.point.y / (self._lookahead * self._lookahead) self._twist.twist.angular.z = curvature * self._desired_velocity return control_robot return "Recovery" # Must have be in a recovery mode # Method to calculate the lookahead point and twist given arc segment from prior to target def calculateArcTwist(self, local_prior, local_target): # Transform the relevant points into the odometry frame self._center.header.stamp = self._last_odom.header.stamp local_center = self._center #self.transformOdom(self._center) if (self._failed): self._done = 'failed' return None # Format for the equations xa = local_center.point.x ya = local_center.point.y ra = self._radius xr = self._current_position.point.x yr = self._current_position.point.y rl = self._lookahead # Calculate the discriminant used in x- and y- calculations xd = (-((ra - rl)**2 - (xa - xr)**2 - (ya - yr)**2) * ((ra + rl)**2 - (xa - xr)**2 - (ya - yr)**2) * (ya - yr)**2 ) # discriminant for x solution yd = (-(ra**4 + (-rl**2 + (xa - xr)**2 + (ya - yr)**2)**2 - 2 * ra**2 * (rl**2 + (xa - xr)**2 + (ya - yr)**2)) * (ya - yr)**2) discrim = np.min((xd, yd)) if (discrim < 0.0): # No intersection - this should not be unless bad start if (self._recover_mode): # Attempt to regain path by driving toward the chord defining the arc pv = Vector3(local_target.point.x - local_prior.point.x, local_target.point.y - local_prior.point.y, 0.0) qv = Vector3( local_prior.point.x - self._current_position.point.x, local_prior.point.y - self._current_position.point.y, 0.0) if (not self.recoverPath(local_target, local_prior, pv, qv)): Logger.logwarn( ' Path recovery failed for arc target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) rrad=%f center=(%f,%f) crad=%f discrim: xd=%f yd=%f ' % (local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, rl, xa, ya, ra, xd, yd)) self._done = 'failed' return None else: Logger.logwarn( ' No path recovery - no intersection for arc target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) rrad=%f center=(%f,%f) crad=%f discrim: xd=%f yd=%f ' % (local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, rl, xa, ya, ra, xd, yd)) self._done = 'failed' return None else: # solve quadratic equation for intersection points xp = (1. / (2 * ((xa - xr)**2 + (ya - yr)**2)) ) # x solution prefix x1 = xp * (rl**2 * (xa - xr) + ra**2 * (-xa + xr) + (xa + xr) * ((xa - xr)**2 + (ya - yr)**2) - np.sqrt(xd)) x2 = xp * (rl**2 * (xa - xr) + ra**2 * (-xa + xr) + (xa + xr) * ((xa - xr)**2 + (ya - yr)**2) + np.sqrt(xd)) y1 = (-ra**2 * (ya - yr)**2 + (xa - xr) * np.sqrt(yd) + (ya - yr) * (rl**2 * (ya - yr) + ((xa - xr)**2 + (ya - yr)**2) * (ya + yr))) / (2 * ((xa - xr)**2 + (ya - yr)**2) * (ya - yr)) y2 = (-ra**2 * (ya - yr)**2 + (xr - xa) * np.sqrt(yd) + (ya - yr) * (rl**2 * (ya - yr) + ((xa - xr)**2 + (ya - yr)**2) * (ya + yr))) / (2 * ((xa - xr)**2 + (ya - yr)**2) * (ya - yr)) # Choose the intersection closest to the target point # All vectors on circle should have same radius from center. A*B = |A||B|Cos(theta) dt = np.array( [local_target.point.x - xa, local_target.point.y - ya]) dp = np.array([local_prior.point.x - xa, local_prior.point.y - ya]) d1 = np.array([x1 - xa, y1 - ya]) d2 = np.array([x2 - xa, y2 - ya]) ap = np.dot(dt, dp) # > implies smaller angle to target a1 = np.dot(dt, d1) a2 = np.dot(dt, d2) control = deepcopy(local_prior) if (ap > a1 and ap > a2): # Intersections must be before the prior point pv = Vector3(local_target.point.x - local_prior.point.x, local_target.point.y - local_prior.point.y, 0.0) qv = Vector3( local_prior.point.x - self._current_position.point.x, local_prior.point.y - self._current_position.point.y, 0.0) if (self._recover_mode): # Attempt to regain path by driving toward the chord defining the arc if (not self.recoverPath(local_target, local_prior, pv, qv)): Logger.logwarn( ' Path recovery failed for arc target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) center=(%f,%f) pv=(%f,%f) qv=(%f,%f) discrim: xd=%f yd=%f ' % (local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, xa, ya, pv.x, pv.y, qv.x, qv.y, xd, yd)) Logger.logwarn( " dt=(%f, %f) dp=(%f, %f) d1=(%f, %f) d2=(%f, %f) ap=%f a1=%f a2=%f" % (dt[0], dt[1], dp[0], dp[1], d1[0], d1[1], d2[0], d2[1], ap, a1, a2)) self._done = 'failed' return None else: # Check to see if we are close to prior in case of perturbation due to localization dp1 = np.dot(dp, d1) dp1 = dp1 / (np.linalg.norm(dp) * np.linalg.norm(d1)) dp2 = np.dot(dp, d2) dp2 = dp2 / (np.linalg.norm(dp) * np.linalg.norm(d2)) dpm = np.max((dp1, dp2)) if (dpm > 0.9): Logger.logwarn( 'Before prior but close, hold current motion! target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) center=(%f,%f) pv=(%f,%f) qv=(%f,%f) discrim: xd=%f yd=%f ' % (local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, xa, ya, pv.x, pv.y, qv.x, qv.y, xd, yd)) Logger.logwarn( " dt=(%f, %f) dp=(%f, %f) d1=(%f, %f) d2=(%f, %f) ap=%f a1=%f a2=%f dp1=%f dp2=%f (%f)" % (dt[0], dt[1], dp[0], dp[1], d1[0], d1[1], d2[0], d2[1], ap, a1, a2, dp1, dp2, dpm)) return None else: Logger.logwarn( ' No path recovery - no valid intersection for arc target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) center=(%f,%f) pv=(%f,%f) qv=(%f,%f) discrim: xd=%f yd=%f ' % (local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, xa, ya, pv.x, pv.y, qv.x, qv.y, xd, yd)) Logger.logwarn( " dt=(%f, %f) dp=(%f, %f) d1=(%f, %f) d2=(%f, %f) ap=%f a1=%f a2=%f dp1=%f dp2=%f (%f)" % (dt[0], dt[1], dp[0], dp[1], d1[0], d1[1], d2[0], d2[1], ap, a1, a2, dp1, dp2, dpm)) self._done = 'failed' return None elif (a1 > a2): # Use intersection 1 control.point.x = x1 control.point.y = y1 else: # Use intersection 2 control.point.x = x2 control.point.y = y2 self._reference_marker.pose.position = deepcopy(control.point) self._local_target_marker.pose.position = deepcopy( local_target.point) control_robot = self.transformBody(control) curvature = 2.0 * control_robot.point.y / (self._lookahead * self._lookahead) self._twist.twist.angular.z = curvature * self._desired_velocity return control_robot return "Recovery" # Must have be in a recovery mode
def __init__(self, desired_velocity=0.2, max_rotation_rate=10.0, target_frame='map', target_x=1.0, target_y=0.1, target_type='line', lookahead_distance=0.25, timeout=0.08, recover_mode=False, center_x=0.0, center_y=0.0, cmd_topic='/create_node/cmd_vel', sensor_topic='/create_node/sensor_state', odometry_topic='/create_node/odom', marker_topic='/pure_pursuit_marker', marker_size=0.05): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(CreatePurePursuitState, self).__init__(outcomes=['done', 'failed'], input_keys=['prior_point'], output_keys=['target_point']) # Store state parameter for later use. self._twist = TwistStamped() self._twist.twist.linear.x = desired_velocity self._twist.twist.angular.z = 0.0 self._desired_velocity = desired_velocity self._max_rotation_rate = max_rotation_rate # Used to clamp the rotation calculations self._current_position = PointStamped() self._current_position.header.stamp = rospy.Time.now() self._current_position.header.frame_id = target_frame self._target = PointStamped() self._target.header.stamp = rospy.Time.now() self._target.header.frame_id = target_frame self._target.point = Point(target_x, target_y, 0.0) self._center = PointStamped() self._center.header = self._target.header self._center.point = Point(center_x, center_y, 0.0) self._lookahead = lookahead_distance self._recover_mode = recover_mode self._target_type = target_type self._target_frame = target_frame if (self._target_type == 'arc'): self._radius = np.sqrt((center_x - target_x)**2 + (center_y - target_y)**2) Logger.loginfo( 'Using arc type with center=(%d, %d) target=(%d,%d) radius=%d' % (self._center.point.x, self._center.point.y, self._target.point.x, self._target.point.y, self._radius)) self._odom_frame = 'odom' # Update with the first odometry message self._robot_frame = 'base_footprint' self._failed = False self._timeout = rospy.Duration(timeout) # transform timeout # The constructor is called when building the state machine, not when actually starting the behavior. # Thus, we cannot save the starting time now and will do so later. self._start_time = None self._done = None # Track the outcome so we can detect if transition is blocked self._odom_topic = odometry_topic self._sensor_topic = sensor_topic self._marker_topic = marker_topic self._cmd_topic = cmd_topic self._listener = ProxyTransformListener() self._sensor_sub = ProxySubscriberCached( {self._sensor_topic: CreateSensorState}) self._odom_sub = ProxySubscriberCached({self._odom_topic: Odometry}) self._pub = ProxyPublisher({self._cmd_topic: TwistStamped}) if (self._marker_topic != ""): self._marker_pub = ProxyPublisher({self._marker_topic: Marker}) else: self._marker_pub = None self._marker = Marker() self._marker.header.frame_id = self._target_frame self._marker.header.stamp = rospy.Time.now() self._marker.ns = "pure_pursuit_waypoints" self._marker.id = int(target_x * 1000000) + int(target_y * 1000) self._marker.type = Marker.SPHERE self._marker.action = Marker.ADD self._marker.pose.position.x = target_x self._marker.pose.position.y = target_y self._marker.pose.position.z = 0.0 self._marker.pose.orientation.x = 0.0 self._marker.pose.orientation.y = 0.0 self._marker.pose.orientation.z = 0.0 self._marker.pose.orientation.w = 1.0 self._marker.scale.x = marker_size self._marker.scale.y = marker_size self._marker.scale.z = marker_size self._marker.color.a = 1.0 # Don't forget to set the alpha! self._marker.color.r = 0.0 self._marker.color.g = 0.0 self._marker.color.b = 1.0 self._reference_marker = Marker() self._reference_marker.header.frame_id = self._target_frame self._reference_marker.header.stamp = rospy.Time.now() self._reference_marker.ns = "pure_pursuit_reference" self._reference_marker.id = 1 self._reference_marker.type = Marker.SPHERE self._reference_marker.action = Marker.ADD self._reference_marker.pose.position.x = target_x self._reference_marker.pose.position.y = target_y self._reference_marker.pose.position.z = 0.0 self._reference_marker.pose.orientation.x = 0.0 self._reference_marker.pose.orientation.y = 0.0 self._reference_marker.pose.orientation.z = 0.0 self._reference_marker.pose.orientation.w = 1.0 self._reference_marker.scale.x = marker_size * 0.75 self._reference_marker.scale.y = marker_size * 0.75 self._reference_marker.scale.z = marker_size * 0.75 self._reference_marker.color.a = 0.0 # Add, but make invisible at first self._reference_marker.color.r = 1.0 self._reference_marker.color.g = 0.0 self._reference_marker.color.b = 1.0 self._local_target_marker = Marker() self._local_target_marker.header.frame_id = self._target_frame self._local_target_marker.header.stamp = rospy.Time.now() self._local_target_marker.ns = "pure_pursuit_target" self._local_target_marker.id = 1 self._local_target_marker.type = Marker.SPHERE self._local_target_marker.action = Marker.ADD self._local_target_marker.pose.position.x = target_x self._local_target_marker.pose.position.y = target_y self._local_target_marker.pose.position.z = 0.0 self._local_target_marker.pose.orientation.x = 0.0 self._local_target_marker.pose.orientation.y = 0.0 self._local_target_marker.pose.orientation.z = 0.0 self._local_target_marker.pose.orientation.w = 1.0 self._local_target_marker.scale.x = marker_size self._local_target_marker.scale.y = marker_size self._local_target_marker.scale.z = marker_size self._local_target_marker.color.a = 0.0 # Add, but make invisible at first self._local_target_marker.color.r = 1.0 self._local_target_marker.color.g = 0.0 self._local_target_marker.color.b = 1.0
class PurePursuitState(EventState): """ This state calculates the twist required to follow a constant curvature arc to the pure pursuit intersection point. The command is published as a TwistStamped command based on parameters. If arc motion is used, the arc should be less than or equal to pi/2 radians. Use multiple targets for longer arcs. -- desired_velocity float Desired velocity in m/s (default: 0.2) -- max_rotation_rate float Maximum rotation rate radians/s (default: 10.0) -- target_frame: string Coordinate frame of target point (default: 'map') -- target_x: float Target point x-coordinate (m) -- target_y: float Target point y-coordinate (m) -- target_type: string Desired motion ('line','arc') (default: 'line') -- lookahead_distance: float Lookahead distance (m) (default: 0.25) -- timeout float Transform timeout (seconds) (default: 0.08) -- recover_mode bool Recover path (typically on startup) (default: False) -- center_x: float Center point x-coordinate for circle defining arc motion (default: 0.0) -- center_y: float Center point y-coordinate for circle defining arc motion (default: 0.0) -- cmd_topic string topic name of the robot command (default: '/create_node/cmd_vel') -- odometry_topic: string topic of the iRobot Create sensor state (default: '/create_node/odom' -- marker_topic: string topic of the RViz marker used for visualization (default: '/pure_pursuit_marker') -- marker_size: float Size of RViz marker used for target (default: 0.05) ># indice int The index ># path Path The path #> indice int The index + 1 #> plan Path The path <= done Reached the end of target relevance <= continue Continue following the path <= failed A bumper was activated prior to completion """ def __init__(self, desired_velocity=0.2, max_rotation_rate=10.0, target_frame='map', target_x=1.0, target_y=0.1, target_type='line', lookahead_distance=0.25, timeout=0.08, recover_mode=False, center_x=0.0, center_y=0.0, cmd_topic='cmd_vel', odometry_topic='odom', marker_topic='pure_pursuit_marker', marker_size=0.05): # Declare outcomes, input_keys, and output_keys by calling the super constructor with the corresponding arguments. super(PurePursuitState, self).__init__(outcomes=['done', 'continue', 'failed'], input_keys=['indice', 'plan'], output_keys=['indice', 'plan']) # Store state parameter for later use. self._twist = TwistStamped() self._twist.twist.linear.x = desired_velocity self._twist.twist.angular.z = 0.0 self._desired_velocity = desired_velocity self._max_rotation_rate = max_rotation_rate # Used to clamp the rotation calculations self._current_position = PointStamped() self._current_position.header.stamp = rospy.Time.now() self._current_position.header.frame_id = target_frame self._target = PointStamped() self._target.header.stamp = rospy.Time.now() self._target.header.frame_id = target_frame self._target.point = Point(target_x, target_y, 0.0) self._prior = PointStamped() self._prior.header.stamp = rospy.Time.now() self._prior.header.frame_id = target_frame self._lookahead = lookahead_distance self._recover_mode = recover_mode self._target_type = target_type self._target_frame = target_frame if (self._target_type == 'arc'): self._radius = np.sqrt((center_x - target_x)**2 + (center_y - target_y)**2) Logger.loginfo( 'Using arc type with center=(%d, %d) target=(%d,%d) radius=%d' % (self._center.point.x, self._center.point.y, self._target.point.x, self._target.point.y, self._radius)) self._odom_frame = 'odom' # Update with the first odometry message self._robot_frame = 'base_footprint' self._failed = False self._timeout = rospy.Duration(timeout) # transform timeout # The constructor is called when building the state machine, not when actually starting the behavior. # Thus, we cannot save the starting time now and will do so later. self._start_time = None self._return = None # Track the outcome so we can detect if transition is blocked self._odom_topic = odometry_topic self._marker_topic = marker_topic self._cmd_topic = cmd_topic self._listener = ProxyTransformListener() self._odom_sub = ProxySubscriberCached({self._odom_topic: Odometry}) self._pub = ProxyPublisher({self._cmd_topic: TwistStamped}) if (self._marker_topic != ""): self._marker_pub = ProxyPublisher({self._marker_topic: Marker}) else: self._marker_pub = None self._marker = Marker() self._marker.header.frame_id = self._target_frame self._marker.header.stamp = rospy.Time.now() self._marker.ns = "pure_pursuit_waypoints" self._marker.id = int(target_x * 1000000) + int(target_y * 1000) self._marker.type = Marker.SPHERE self._marker.action = Marker.ADD self._marker.pose.position.x = target_x self._marker.pose.position.y = target_y self._marker.pose.position.z = 0.0 self._marker.pose.orientation.x = 0.0 self._marker.pose.orientation.y = 0.0 self._marker.pose.orientation.z = 0.0 self._marker.pose.orientation.w = 1.0 self._marker.scale.x = marker_size self._marker.scale.y = marker_size self._marker.scale.z = marker_size self._marker.color.a = 1.0 # Don't forget to set the alpha! self._marker.color.r = 0.0 self._marker.color.g = 0.0 self._marker.color.b = 1.0 self._reference_marker = Marker() self._reference_marker.header.frame_id = self._target_frame self._reference_marker.header.stamp = rospy.Time.now() self._reference_marker.ns = "pure_pursuit_reference" self._reference_marker.id = 1 self._reference_marker.type = Marker.SPHERE self._reference_marker.action = Marker.ADD self._reference_marker.pose.position.x = target_x self._reference_marker.pose.position.y = target_y self._reference_marker.pose.position.z = 0.0 self._reference_marker.pose.orientation.x = 0.0 self._reference_marker.pose.orientation.y = 0.0 self._reference_marker.pose.orientation.z = 0.0 self._reference_marker.pose.orientation.w = 1.0 self._reference_marker.scale.x = marker_size * 0.75 self._reference_marker.scale.y = marker_size * 0.75 self._reference_marker.scale.z = marker_size * 0.75 self._reference_marker.color.a = 0.0 # Add, but make invisible at first self._reference_marker.color.r = 1.0 self._reference_marker.color.g = 0.0 self._reference_marker.color.b = 1.0 self._local_target_marker = Marker() self._local_target_marker.header.frame_id = self._target_frame self._local_target_marker.header.stamp = rospy.Time.now() self._local_target_marker.ns = "pure_pursuit_target" self._local_target_marker.id = 1 self._local_target_marker.type = Marker.SPHERE self._local_target_marker.action = Marker.ADD self._local_target_marker.pose.position.x = target_x self._local_target_marker.pose.position.y = target_y self._local_target_marker.pose.position.z = 0.0 self._local_target_marker.pose.orientation.x = 0.0 self._local_target_marker.pose.orientation.y = 0.0 self._local_target_marker.pose.orientation.z = 0.0 self._local_target_marker.pose.orientation.w = 1.0 self._local_target_marker.scale.x = marker_size self._local_target_marker.scale.y = marker_size self._local_target_marker.scale.z = marker_size self._local_target_marker.color.a = 0.0 # Add, but make invisible at first self._local_target_marker.color.r = 1.0 self._local_target_marker.color.g = 0.0 self._local_target_marker.color.b = 1.0 # Transform point into odometry frame def transformOdom(self, point): try: # Get transform self._listener.listener().waitForTransform(self._odom_frame, point.header.frame_id, point.header.stamp, self._timeout) return self._listener.listener().transformPoint( self._odom_frame, point) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: Logger.logwarn('Failed to get the transformation\n%s' % str(e)) self._failed = True return None except Exception as e: Logger.logwarn( 'Failed to get the transformation due to unknown error\n %s' % str(e)) self._failed = True return None # Transform point into map frame def transformMap(self, odometry): odom_position = PointStamped() odom_position.header = odometry.header odom_position.point = odometry.pose.pose.position try: # Get transform self._listener.listener().waitForTransform( self._target_frame, odometry.header.frame_id, odometry.header.stamp, self._timeout) return self._listener.listener().transformPoint( self._target_frame, odom_position) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: Logger.logwarn( 'Failed to get the transformation to target_frame\n%s' % str(e)) self._failed = True return None except Exception as e: Logger.logwarn( 'Failed to get the transformation to target frame due to unknown error\n %s' % str(e)) self._failed = True return None # Transform point into robot body frame def transformBody(self, point): try: # Get transform self._listener.listener().waitForTransform(self._robot_frame, point.header.frame_id, point.header.stamp, self._timeout) return self._listener.listener().transformPoint( self._robot_frame, point) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: Logger.logwarn('Failed to get the transformation\n%s' % str(e)) self._failed = True return None except: Logger.logwarn( 'Failed to get the transformation due to unknown error\n') self._failed = True return None def execute(self, userdata): # This method is called periodically while the state is active. # If no outcome is returned, the state will stay active. if (self._return): # We have completed the state, and therefore must be blocked by autonomy level # Stop the robot, but and return the prior outcome ts = TwistStamped() ts.header.stamp = rospy.Time.now() self._pub.publish(self._cmd_topic, ts) return self._return # Get the latest odometry data if (self._sub.has_msg(self._odom_topic)): self._last_odom = self._sub.get_last_msg(self._odom_topic) # Update the current pose self._current_position = self.transformMap(self._last_odom) if (self._failed): self._return = 'failed' return 'failed' # Transform the target points into the current odometry frame self._target.header.stamp = self._last_odom.header.stamp local_target = self._target #self.transformOdom(self._target) # If target point is withing lookahead distance then we are done dr = np.sqrt( (local_target.point.x - self._current_position.point.x)**2 + (local_target.point.y - self._current_position.point.y)**2) if (dr < self._lookahead): Logger.loginfo( ' %s Lookahead circle is past target - done! target=(%f, %f, %f) robot=(%f,%f, %f) dr=%f lookahead=%f ' % (self.name, local_target.point.x, local_target.point.y, local_target.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, dr, self._lookahead)) if (userdata.indice == len(userdata.plan.poses) - 1): self._return = 'done' return 'done' else: self._return = 'continue' return 'continue' # Transform the prior target point into the current odometry frame self._prior.header.stamp = self._last_odom.header.stamp local_prior = self._prior #self.transformOdom(self._prior) if (self._failed): self._return = 'failed' return 'failed' # Assume we can go the desired velocity self._twist.twist.linear.x = self._desired_velocity lookahead = None lookahead = self.calculateLineTwist(local_prior, local_target) if (lookahead is None): # Did not get a lookahead point so we either failed, completed segment, or are deliberately holding the prior velocity (to recover from minor perturbations) if (self._return is not None): Logger.logerr("No lookahead!") return self._return # return what was set (either 'failed' or 'done') # Sanity check the rotation rate if (math.fabs(self._twist.twist.angular.z) > self._max_rotation_rate): self._twist.twist.linear.x = self._desired_velocity * self._max_rotation_rate / math.fabs( self._twist.twist.angular.z) # decrease the speed self._twist.twist.angular.z = math.copysign( self._max_rotation_rate, self._twist.twist.angular.z) # Normal operation - publish the latest calculated twist self._twist.header.stamp = rospy.Time.now() self._pub.publish(self._cmd_topic, self._twist) if (self._marker_pub): self._marker_pub.publish(self._marker_topic, self._reference_marker) self._marker_pub.publish(self._marker_topic, self._local_target_marker) return None def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. self._start_time = rospy.Time.now() self._return = None # reset the completion flag self._failed = False # reset the failed flag if (self._marker_pub): self._marker.action = Marker.MODIFY self._marker.color.a = 1.0 self._marker.color.r = 0.0 self._marker.color.g = 1.0 # Indicate this target is active self._marker.color.b = 0.0 self._marker_pub.publish(self._marker_topic, self._marker) if (userdata.indice > 0 and userdata.indice < len(userdata.plan.poses)): Logger.loginfo(" Access data for index %d" % (userdata.indice)) self._target.point = Point( userdata.plan.poses[userdata.indice].pose.position.x, userdata.plan.poses[userdata.indice].pose.position.y, 0.0) self._prior.point = Point( userdata.plan.poses[userdata.indice - 1].pose.position.x, userdata.plan.poses[userdata.indice - 1].pose.position.y, 0.0) Logger.loginfo( " Moving toward target %d =%f,%f from prior=%f,%f" % (userdata.indice, self._target.point.x, self._target.point.y, self._prior.point.x, self._prior.point.y)) else: Logger.logerr( " Invalid index %d - cannot access the path points!" % (userdata.indice)) self._target.point = None self._prior.point = None self._failed = True self._return = 'failed' # Increment the index for the next segment userdata.indice += 1 def on_exit(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. elapsed_time = rospy.Time.now() - self._start_time # userdata.target_point = self._target # Set the user data for passing to next node #Logger.logdebug(" Spent %f seconds in this segment target=%f,%f position=%f,%f" % # (elapsed_time.to_sec(),self._target.point.x, self._target.point.y, # self._current_position.point.x, self._current_position.point.y)) if (self._marker_pub): self._marker.color.a = 1.0 # Don't forget to set the alpha! self._marker.color.r = 0.8 # Indicate this target is no longer active self._marker.color.g = 0.0 self._marker.color.b = 0.0 self._marker_pub.publish(self._marker_topic, self._marker) def on_start(self): self._return = None # Clear completion flag # Wait for odometry message while (not self._odom_sub.has_msg(self._odom_topic)): Logger.logwarn('Waiting for odometry message from the robot ') rospy.sleep(0.05) while (not self._odom_sub.has_msg(self._odom_topic)): Logger.logwarn( 'Waiting for odometry message to become available from the robot ' ) rospy.sleep(0.25) self._last_odom = self._sub.get_last_msg(self._odom_topic) self._odom_frame = self._last_odom.header.frame_id #Logger.loginfo(' odometry frame id <%s>' % (self._odom_frame)) # Update the target transformation self._target.header.stamp = self._last_odom.header.stamp while (self.transformOdom(self._target) is None): Logger.logwarn( 'Waiting for tf transformations to odometry frame to become available from the robot ' ) rospy.sleep(0.25) self._target.header.stamp = rospy.Time.now() while (self.transformMap(self._last_odom) is None): Logger.logwarn( 'Waiting for tf transformations to map frame become available from the robot ' ) rospy.sleep(0.25) self._last_odom = self._sub.get_last_msg(self._odom_topic) self._current_position = self.transformMap(self._last_odom) # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. if (self._marker_pub): self._marker.action = Marker.ADD self._marker.color.a = 1.0 # Set alpha otherwise the marker is invisible self._marker.color.r = 0.0 self._marker.color.g = 0.0 self._marker.color.b = 1.0 # Indicate this target is planned self._marker_pub.publish(self._marker_topic, self._marker) self._marker_pub.publish(self._marker_topic, self._reference_marker) self._marker_pub.publish(self._marker_topic, self._local_target_marker) self._marker.action = Marker.MODIFY self._reference_marker.action = Marker.MODIFY self._reference_marker.color.a = 1.0 # Set alpha so it will become visible on next publish self._local_target_marker.action = Marker.MODIFY self._local_target_marker.color.a = 1.0 # Set alpha so it will become visible on next publish # Method to calculate the lookahead point given line segment from prior to target def calculateLineTwist(self, local_prior, local_target): # Define a line segment from prior to the target (assume 2D) pv = Vector3(local_target.point.x - local_prior.point.x, local_target.point.y - local_prior.point.y, 0.0) qv = Vector3(local_prior.point.x - self._current_position.point.x, local_prior.point.y - self._current_position.point.y, 0.0) # Find intersection of line segment with lookahead circle centered at the robot a = pv.x * pv.x + pv.y * pv.y # b = 2.0 * (qv.x * pv.x + qv.y * pv.y) c = (qv.x * qv.x + qv.y * qv.y) - self._lookahead * self._lookahead if (a < 0.001): Logger.logerr( ' %s Invalid prior and target for line: target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f ' % (self.name, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c)) self._return = 'failed' return None discrim = b * b - 4 * a * c if (discrim < 0.0): # No intersection - this should not be unless bad start or localization perturbation Logger.logwarn( ' %s No path recovery - no intersection for line: target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._return = 'failed' return None else: # solve quadratic equation for intersection points sqd = math.sqrt(discrim) t1 = (-b - sqd) / (2 * a) # min value t2 = (-b + sqd) / (2 * a) # max value if (t2 < t1): Logger.logwarn( ' %s Say what! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._return = 'failed' return None if (t2 < 0.0): # all intersections are behind the segment - this should not be unless bad start or localization perturbation if (t2 > -0.1): # likely due to localization perturbation Logger.logwarn( ' %s Circle is before segment - continue prior motion! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) return None else: Logger.logwarn( ' %s Circle is before segment! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._return = 'failed' return None elif (t1 > 1.0): # all intersections are past the segment Logger.loginfo( ' %s Circle is past segment - done! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._return = 'done' return None elif (t1 < 0.0 and t2 > 1.0): # Segment is contained inside the lookahead circle Logger.loginfo( ' %s Circle contains segment - move along! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._return = 'done' return None elif (t2 > 1.0): # The lookahead circle extends beyond the target point - we are finished here Logger.loginfo( ' %s Circle extends past segment - done! t1=%f t2=%f sqd=%f target=(%f, %f, %f) prior=(%f, %f, %f) robot=(%f,%f, %f) pv=(%f,%f) qv=(%f,%f) a=%f b=%f c=%f discrim=%f ' % (self.name, t1, t2, sqd, local_target.point.x, local_target.point.y, local_target.point.z, local_prior.point.x, local_prior.point.y, local_prior.point.z, self._current_position.point.x, self._current_position.point.y, self._current_position.point.z, pv.x, pv.y, qv.x, qv.y, a, b, c, discrim)) self._return = 'done' return None else: # This is the normal case # Must be line segment control = deepcopy(local_prior) control.point.x = control.point.x + t2 * pv.x # Control point in the odometry frame control.point.y = control.point.y + t2 * pv.y # Control point in the odometry frame control.point.z = control.point.z + t2 * pv.z # Control point in the odometry frame self._reference_marker.pose.position = control.point self._local_target_marker.pose.position = local_target.point control_robot = self.transformBody(control) curvature = 2.0 * control_robot.point.y / (self._lookahead * self._lookahead) self._twist.twist.angular.z = curvature * self._desired_velocity return control_robot return None