def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
q = msg.orientation
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.quat_des = q
self.initialized = True
# Compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
# Error quaternion wrt body frame
e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q),
self.quat_des)
# Error angles
e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat))
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0],
y=v_angular[1],
z=v_angular[2])
self.pub_cmd_vel.publish(cmd_vel)
def odometry_callback(msg, name):
global vehicle_pub
if name not in vehicle_pub:
return
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
orientation = np.array([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
yaw = euler_from_quaternion(orientation)[2]
new_marker = deepcopy(marker)
points = list()
for i in range(new_marker.shape[0]):
new_marker[i, :] = np.dot(rot(yaw - np.pi / 2), new_marker[i, :])
new_marker[i, 0] += x
new_marker[i, 1] += y
p = Point32()
p.x = new_marker[i, 0]
p.y = new_marker[i, 1]
points.append(p)
new_poly = PolygonStamped()
new_poly.header.stamp = rospy.Time.now()
new_poly.header.frame_id = 'world'
new_poly.polygon.points = points
vehicle_pub[name].publish(new_poly)
def set_waypoints(self, waypoints, init_rot=(0, 0, 0, 1)):
"""Initializes the waypoint interpolator with a set of waypoints."""
self._logger.info('Initial rotation vector (quat)=%s', str(init_rot))
self._logger.info('Initial rotation vector (rpy)=%s',
str(euler_from_quaternion(init_rot)))
if self._wp_interp.init_waypoints(waypoints, init_rot):
self._wp_interp_on = True
return True
else:
return False
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
p = msg.pose.pose.position
q = msg.pose.pose.orientation
p = numpy.array([p.x, p.y, p.z])
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.pos_des = p
self.quat_des = q
self.initialized = True
# Compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
# Position error
e_pos_world = self.pos_des - p
e_pos_body = transf.quaternion_matrix(q).transpose()[0:3, 0:3].dot(
e_pos_world)
# Error quaternion wrt body frame
e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q),
self.quat_des)
if numpy.linalg.norm(e_pos_world[0:2]) > 5.0:
# special case if we are far away from goal:
# ignore desired heading, look towards goal position
heading = math.atan2(e_pos_world[1], e_pos_world[0])
quat_des = numpy.array(
[0, 0, math.sin(0.5 * heading),
math.cos(0.5 * heading)])
e_rot_quat = transf.quaternion_multiply(
transf.quaternion_conjugate(q), quat_des)
# Error angles
e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat))
v_linear = self.pid_pos.regulate(e_pos_body, t)
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.linear = geometry_msgs.Vector3(x=v_linear[0],
y=v_linear[1],
z=v_linear[2])
cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0],
y=v_angular[1],
z=v_angular[2])
self.pub_cmd_vel.publish(cmd_vel)
def odometry_callback(self, msg):
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
orientation = np.array([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
yaw = euler_from_quaternion(orientation)[2]
# Generating the vehicle footprint marker
new_marker = self._scale_footprint * deepcopy(self.MARKER)
points = list()
for i in range(new_marker.shape[0]):
new_marker[i, :] = np.dot(self.rot(yaw - np.pi / 2),
new_marker[i, :])
new_marker[i, 0] += x
new_marker[i, 1] += y
p = Point32()
p.x = new_marker[i, 0]
p.y = new_marker[i, 1]
points.append(p)
new_poly = PolygonStamped()
new_poly.header.stamp = self.get_clock().now() #rospy.Time.now()
new_poly.header.frame_id = 'world'
new_poly.polygon.points = points
self._footprint_pub.publish(new_poly)
# Generating the label marker
self._label_marker.pose.position.x = msg.pose.pose.position.x + self._label_x_offset
self._label_marker.pose.position.y = msg.pose.pose.position.y
self._label_marker.pose.position.z = msg.pose.pose.position.z
self._label_pub.publish(self._label_marker)
def rot(self):
"""`numpy.array`: `roll`, `pitch` and `yaw` angles"""
rpy = euler_from_quaternion(self._rot)
return np.array([rpy[0], rpy[1], rpy[2]])
def odometry_callback(self, msg):
"""Handle odometry callback: The actual control loop."""
# Update local planner's vehicle position and orientation
pos = [
msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z
]
quat = [
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
]
self.local_planner.update_vehicle_pose(pos, quat)
# Compute the desired position
t = time_in_float_sec(self.get_clock().now())
des = self.local_planner.interpolate(t)
# Publish the reference
ref_msg = TrajectoryPoint()
ref_msg.header.stamp = self.get_clock().now().to_msg()
ref_msg.header.frame_id = self.local_planner.inertial_frame_id
ref_msg.pose.position = geometry_msgs.Point(**self.to_dict_vect3(
*des.p))
ref_msg.pose.orientation = geometry_msgs.Quaternion(
**self.to_dict_quat(*des.q))
ref_msg.velocity.linear = geometry_msgs.Vector3(**self.to_dict_vect3(
*des.vel[0:3]))
ref_msg.velocity.angular = geometry_msgs.Vector3(**self.to_dict_vect3(
*des.vel[3::]))
self.reference_pub.publish(ref_msg)
p = self.vector_to_np(msg.pose.pose.position)
forward_vel = self.vector_to_np(msg.twist.twist.linear)
ref_vel = des.vel[0:3]
q = self.quaternion_to_np(msg.pose.pose.orientation)
rpy = euler_from_quaternion(q, axes='sxyz')
# Compute tracking errors wrt world frame:
e_p = des.p - p
abs_pos_error = np.linalg.norm(e_p)
abs_vel_error = np.linalg.norm(ref_vel - forward_vel)
# Generate error message
error_msg = TrajectoryPoint()
error_msg.header.stamp = self.get_clock().now().to_msg()
error_msg.header.frame_id = self.local_planner.inertial_frame_id
error_msg.pose.position = geometry_msgs.Point(**self.to_dict_vect3(
*e_p))
error_msg.pose.orientation = geometry_msgs.Quaternion(
**self.to_dict_quat(
*quaternion_multiply(quaternion_inverse(q), des.q)))
error_msg.velocity.linear = geometry_msgs.Vector3(**self.to_dict_vect3(
*(des.vel[0:3] - self.vector_to_np(msg.twist.twist.linear))))
error_msg.velocity.angular = geometry_msgs.Vector3(
**self.to_dict_vect3(
*(des.vel[3::] - self.vector_to_np(msg.twist.twist.angular))))
# Based on position tracking error: Compute desired orientation
pitch_des = -math.atan2(e_p[2], np.linalg.norm(e_p[0:2]))
# Limit desired pitch angle:
pitch_des = max(-self.desired_pitch_limit,
min(pitch_des, self.desired_pitch_limit))
yaw_des = math.atan2(e_p[1], e_p[0])
yaw_err = self.unwrap_angle(yaw_des - rpy[2])
# Limit yaw effort
yaw_err = min(self.yaw_error_limit, max(-self.yaw_error_limit,
yaw_err))
# Roll: P controller to keep roll == 0
roll_control = self.p_roll * rpy[0]
# Pitch: P controller to reach desired pitch angle
pitch_control = self.p_pitch * self.unwrap_angle(
pitch_des - rpy[1]) + self.d_pitch * (des.vel[4] -
msg.twist.twist.angular.y)
# Yaw: P controller to reach desired yaw angle
yaw_control = self.p_yaw * yaw_err + self.d_yaw * (
des.vel[5] - msg.twist.twist.angular.z)
# Limit thrust
thrust = min(
self.max_thrust,
self.p_gain_thrust * np.linalg.norm(abs_pos_error) +
self.d_gain_thrust * abs_vel_error)
thrust = max(self.min_thrust, thrust)
rpy = np.array([roll_control, pitch_control, yaw_control])
# In case the world_ned reference frame is used, the convert it back
# to the ENU convention to generate the reference fin angles
rtf = deepcopy(self.rpy_to_fins)
if self.local_planner.inertial_frame_id == 'world_ned':
rtf[:, 1] *= -1
rtf[:, 2] *= -1
# Transform orientation command into fin angle set points
fins = rtf.dot(rpy)
# Check for saturation
max_angle = max(np.abs(fins))
if max_angle >= self.max_fin_angle:
fins = fins * self.max_fin_angle / max_angle
thrust_force = self.thruster.tam_column * thrust
self.thruster.publish_command(thrust_force[0])
cmd = FloatStamped()
for i in range(self.n_fins):
cmd.header.stamp = self.get_clock().now().to_msg()
cmd.header.frame_id = '%s/fin%d' % (self.namespace, i)
cmd.data = min(fins[i], self.max_fin_angle)
cmd.data = max(cmd.data, -self.max_fin_angle)
self.pub_cmd[i].publish(cmd)
self.error_pub.publish(error_msg)
