def odom_cb(self, msg): self.current_position = tools.position_from_pose(msg.pose.pose) self.current_orientation = tools.orientation_from_pose(msg.pose.pose) # Get distance to the goal vector_to_goal = self.desired_position - self.current_position self.distance_to_goal = np.linalg.norm(vector_to_goal) self.angle_to_goal_orientation = abs((self.desired_orientation % (2 * np.pi)) - (self.current_orientation % (2 * np.pi))) # overshoot is 1 if behind line drawn perpendicular to the goal line and through the desired position, -1 if on the other # Side of said line overshoot = np.dot(vector_to_goal / self.distance_to_goal, self.line.hat) self.overshoot = overshoot / abs(overshoot)
def odom_cb(self, msg):
self.current_position = tools.position_from_pose(msg.pose.pose)
self.current_orientation = tools.orientation_from_pose(msg.pose.pose)
# Get distance to the goal
vector_to_goal = self.desired_position - self.current_position
self.distance_to_goal = np.linalg.norm(vector_to_goal)
self.angle_to_goal_orientation = abs((self.desired_orientation %
(2 * np.pi)) -
(self.current_orientation %
(2 * np.pi)))
# overshoot is 1 if behind line drawn perpendicular to the goal line and through the desired position, -1 if on the other
# Side of said line
overshoot = np.dot(vector_to_goal / self.distance_to_goal,
self.line.hat)
self.overshoot = overshoot / abs(overshoot)
def odom_cb(self, msg):
# Pose is relative to /enu!!!!!!!!
# Twist is relative to /base_link!!!!!!!!!
self.current_position = tools.position_from_pose(msg.pose.pose)
self.current_orientation = tools.orientation_from_pose(msg.pose.pose)
# Transform velocity from boat frame of reference to enu
vec = Vector3Stamped(
header = Header(
frame_id = '/base_link',
stamp = msg.header.stamp + rospy.Duration(-0.1)), # Look a tenth of a second into the past
vector = msg.twist.twist.linear)
try:
#rospy.loginfo('Vec before transform: ' + str(vec))
vec = self.tf_listener.transformVector3('/enu', vec)
#rospy.loginfo('Vec after transform: ' + str(vec))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as err:
#rospy.logwarn('Tf error: ' + str(err))
return
self.current_velocity = xyz_array(vec.vector)
def odom_cb(self, msg):
# Lock on odom cb
self.as_lock.acquire()
#print 'odom lock'
self.current_position = tools.position_from_pose(msg.pose.pose)
# Zero the Z
self.current_position[2] = 0
self.current_orientation = tools.orientation_from_pose(msg.pose.pose)
# Give trajectory generator feedback
if self.traj_gen is not None:
self.traj_gen.feedback(self.get_current_posetwist())
# Get distance to the goal
vector_to_goal = self.desired_position - self.current_position
self.distance_to_goal = np.linalg.norm(vector_to_goal)
self.angle_to_goal_orientation = map(tools.smallest_coterminal_angle, self.desired_orientation - self.current_orientation)
# Release lock
self.as_lock.release()
def odom_cb(self, msg):
# Pose is relative to /enu!!!!!!!!
# Twist is relative to /base_link!!!!!!!!!
self.current_position = tools.position_from_pose(msg.pose.pose)
self.current_orientation = tools.orientation_from_pose(msg.pose.pose)
# Transform velocity from boat frame of reference to enu
vec = Vector3Stamped(
header=Header(frame_id='/base_link',
stamp=msg.header.stamp + rospy.Duration(-0.1)
), # Look a tenth of a second into the past
vector=msg.twist.twist.linear)
try:
#rospy.loginfo('Vec before transform: ' + str(vec))
vec = self.tf_listener.transformVector3('/enu', vec)
#rospy.loginfo('Vec after transform: ' + str(vec))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as err:
#rospy.logwarn('Tf error: ' + str(err))
return
self.current_velocity = xyz_array(vec.vector)
