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