def _update_reference(self):
"""Call the local planner interpolator to retrieve a trajectory
point and publish the reference message as `uuv_control_msgs/TrajectoryPoint`.
"""
# Update the local planner's information about the vehicle's pose
self._local_planner.update_vehicle_pose(self._vehicle_model.pos,
self._vehicle_model.quat)
t = rospy.get_time()
reference = self._local_planner.interpolate(t)
if reference is not None:
self._reference['pos'] = reference.p
self._reference['rot'] = reference.q
self._reference['vel'] = np.hstack((reference.v, reference.w))
self._reference['acc'] = np.hstack((reference.a, reference.alpha))
if reference is not None and self._reference_pub.get_num_connections(
) > 0:
# Publish current reference
msg = TrajectoryPoint()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self._local_planner.inertial_frame_id
msg.pose.position = Vector3(*self._reference['pos'])
msg.pose.orientation = Quaternion(*self._reference['rot'])
msg.velocity.linear = Vector3(*self._reference['vel'][0:3])
msg.velocity.angular = Vector3(*self._reference['vel'][3:6])
msg.acceleration.linear = Vector3(*self._reference['acc'][0:3])
msg.acceleration.angular = Vector3(*self._reference['acc'][3:6])
self._reference_pub.publish(msg)
return True
def _update_reference(self):
# Update the local planner's information about the vehicle's pose
self._local_planner.update_vehicle_pose(self._vehicle_model.pos,
self._vehicle_model.quat)
t = rospy.get_time()
reference = self._local_planner.interpolate(t)
if reference is not None:
self._reference['pos'] = reference.p
self._reference['rot'] = reference.q
self._reference['vel'] = np.hstack((reference.v, reference.w))
self._reference['acc'] = np.hstack((reference.a, reference.alpha))
# Publish current reference
msg = TrajectoryPoint()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'world'
msg.pose.position = Vector3(*self._reference['pos'])
msg.pose.orientation = Quaternion(*self._reference['rot'])
msg.velocity.linear = Vector3(*self._reference['vel'][0:3])
msg.velocity.angular = Vector3(*self._reference['vel'][3:6])
msg.acceleration.linear = Vector3(*self._reference['acc'][0:3])
msg.acceleration.angular = Vector3(*self._reference['acc'][3:6])
self._reference_pub.publish(msg)
return True
def update_errors(self):
"""Update error vectors."""
if not self.odom_is_init:
self._logger.warning('Odometry topic has not been update yet')
return
self._update_reference()
# Calculate error in the BODY frame
self._update_time_step()
# Rotation matrix from INERTIAL to BODY frame
rotItoB = self._vehicle_model.rotItoB
rotBtoI = self._vehicle_model.rotBtoI
if self._dt > 0:
# Update position error with respect to the the BODY frame
pos = self._vehicle_model.pos
vel = self._vehicle_model.vel
quat = self._vehicle_model.quat
self._errors['pos'] = np.dot(rotItoB, self._reference['pos'] - pos)
# Update orientation error
self._errors['rot'] = quaternion_multiply(quaternion_inverse(quat),
self._reference['rot'])
# Velocity error with respect to the the BODY frame
self._errors['vel'] = np.hstack(
(np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
if self._error_pub.get_num_connections() > 0:
stamp = self.get_clock().now().to_msg()
msg = TrajectoryPoint()
msg.header.stamp = stamp
msg.header.frame_id = self._local_planner.inertial_frame_id
# Publish pose error
pos_error = np.dot(rotBtoI, self._errors['pos'])
msg.pose.position = Vector3(x=pos_error[0],
y=pos_error[1],
z=pos_error[2])
msg.pose.orientation = Quaternion(x=self._errors['rot'][0],
y=self._errors['rot'][1],
z=self._errors['rot'][2])
# Publish velocity errors in INERTIAL frame
vel_errors = np.dot(rotBtoI, self._errors['vel'][0:3])
msg.velocity.linear = Vector3(x=vel_errors[0],
y=vel_errors[1],
z=vel_errors[2])
vel_errors = np.dot(rotBtoI, self._errors['vel'][3:6])
msg.velocity.angular = Vector3(x=vel_errors[0],
y=vel_errors[1],
z=vel_errors[2])
# msg.velocity.angular = Vector3(*np.dot(rotBtoI, self._errors['vel'][3:6]))
self._error_pub.publish(msg)
def _update_reference(self):
"""Call the local planner interpolator to retrieve a trajectory
point and publish the reference message as `uuv_control_msgs/TrajectoryPoint`.
"""
# Update the local planner's information about the vehicle's pose
self._local_planner.update_vehicle_pose(self._vehicle_model.pos,
self._vehicle_model.quat)
t = time_in_float_sec(self.get_clock().now())
reference = self._local_planner.interpolate(t)
if reference is not None:
self._reference['pos'] = reference.p
self._reference['rot'] = reference.q
self._reference['vel'] = np.hstack((reference.v, reference.w))
self._reference['acc'] = np.hstack((reference.a, reference.alpha))
if reference is not None and self._reference_pub.get_subscription_count(
) > 0:
# Publish current reference
msg = TrajectoryPoint()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = self._local_planner.inertial_frame_id
msg.pose.position = Vector3(x=self._reference['pos'][0],
y=self._reference['pos'][1],
z=self._reference['pos'][2])
msg.pose.orientation = Quaternion(x=self._reference['rot'][0],
y=self._reference['rot'][1],
z=self._reference['rot'][2],
w=self._reference['rot'][3])
msg.velocity.linear = Vector3(x=self._reference['vel'][0],
y=self._reference['vel'][1],
z=self._reference['vel'][2])
msg.velocity.angular = Vector3(x=self._reference['vel'][3],
y=self._reference['vel'][4],
z=self._reference['vel'][5])
msg.acceleration.linear = Vector3(x=self._reference['acc'][0],
y=self._reference['acc'][1],
z=self._reference['acc'][2])
msg.acceleration.angular = Vector3(x=self._reference['acc'][3],
y=self._reference['acc'][4],
z=self._reference['acc'][5])
self._reference_pub.publish(msg)
return True
def update_errors(self):
if not self.odom_is_init:
self._logger.warning('Odometry topic has not been update yet')
return
self._update_reference()
# Calculate error in the BODY frame
self._update_time_step()
# Rotation matrix from INERTIAL to BODY frame
rotItoB = self._vehicle_model.rotItoB
rotBtoI = self._vehicle_model.rotBtoI
if self._dt > 0:
# Update position error with respect to the the BODY frame
pos = self._vehicle_model.pos
vel = self._vehicle_model.vel
quat = self._vehicle_model.quat
self._errors['pos'] = np.dot(rotItoB, self._reference['pos'] - pos)
# Update orientation error with respect to the BODY frame
self._errors['rot'] = quaternion_multiply(quaternion_inverse(quat),
self._reference['rot'])
# Velocity error with respect to the the BODY frame
self._errors['vel'] = np.hstack(
(np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
stamp = rospy.Time.now()
msg = TrajectoryPoint()
msg.header.stamp = stamp
msg.header.frame_id = 'world'
# Publish pose error
msg.pose.position = Vector3(*np.dot(rotBtoI, self._errors['pos']))
msg.pose.orientation = Quaternion(*self._errors['rot'])
# Publish velocity errors in INERTIAL frame
msg.velocity.linear = Vector3(
*np.dot(rotBtoI, self._errors['vel'][0:3]))
msg.velocity.angular = Vector3(
*np.dot(rotBtoI, self._errors['vel'][3:6]))
self._error_pub.publish(msg)
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.Vector3(*des.p)
ref_msg.pose.orientation = geometry_msgs.Quaternion(*des.q)
ref_msg.velocity.linear = geometry_msgs.Vector3(*des.vel[0:3])
ref_msg.velocity.angular = geometry_msgs.Vector3(*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 = trans.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.Vector3(*e_p)
error_msg.pose.orientation = geometry_msgs.Quaternion(
*trans.quaternion_multiply(trans.quaternion_inverse(q), des.q))
error_msg.velocity.linear = geometry_msgs.Vector3(
*(des.vel[0:3] - self.vector_to_np(msg.twist.twist.linear)))
error_msg.velocity.angular = geometry_msgs.Vector3(
*(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)
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 = rospy.Time.now().to_sec()
des = self.local_planner.interpolate(t)
# Publish the reference
ref_msg = TrajectoryPoint()
ref_msg.header.stamp = rospy.Time.now()
ref_msg.header.frame_id = self.local_planner.inertial_frame_id
ref_msg.pose.position = geometry_msgs.Vector3(*des.p)
ref_msg.pose.orientation = geometry_msgs.Quaternion(*des.q)
self.reference_pub.publish(ref_msg)
p = self.vector_to_numpy(msg.pose.pose.position)
q = self.quaternion_to_numpy(msg.pose.pose.orientation)
R = trans.quaternion_matrix(q)[0:3, 0:3]
v = self.vector_to_numpy(msg.twist.twist.linear)
rpy = trans.euler_from_quaternion(q, axes='sxyz')
# Compute tracking errors wrt world frame:
e_p = des.p - p
n_e_p = numpy.linalg.norm(e_p)
# Generate error message
error_msg = TrajectoryPoint()
error_msg.header.stamp = rospy.Time.now()
error_msg.header.frame_id = self.local_planner.inertial_frame_id
error_msg.pose.position = geometry_msgs.Vector3(*e_p)
error_msg.pose.orientation = geometry_msgs.Quaternion(
*trans.quaternion_multiply(trans.quaternion_inverse(q), des.q))
# Based on position tracking error: Compute desired orientation
pitch_des = -math.atan2(e_p[2], numpy.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.gain_roll * rpy[0]
# Pitch: P controller to reach desired pitch angle
pitch_control = self.gain_pitch * self.unwrap_angle(pitch_des - rpy[1])
# Yaw: P controller to reach desired yaw angle
yaw_control = self.gain_yaw * yaw_err
# Limit thrust
thrust = min(self.max_thrust, self.gain_thrust * n_e_p)
thrust = max(self.min_thrust, thrust)
rpy = numpy.array([roll_control, pitch_control, yaw_control])
# Transform orientation command into fin angle set points
fins = self.rpy_to_fins.dot(rpy)
# Check for saturation
max_angle = max(numpy.abs(fins))
if max_angle >= self.max_fin_angle:
fins = fins * max_angle / self.max_fin_angle
cmd = FloatStamped()
cmd.header.stamp = rospy.Time.now()
cmd.data = -1 * thrust
self.pub_thrust.publish(cmd)
for i in range(self.n_fins):
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)
def update_errors(self):
"""Update error vectors."""
if not self.odom_is_init:
self._logger.warning('Odometry topic has not been update yet')
return
self._update_reference()
self.count()
# Calculate error in the BODY frame
self._update_time_step()
# Rotation matrix from INERTIAL to BODY frame
rotItoB = self._vehicle_model.rotItoB
rotBtoI = self._vehicle_model.rotBtoI
if self._dt > 0:
# Update position error with respect to the the BODY frame
pos = self._vehicle_model.pos
vel = self._vehicle_model.vel
quat = self._vehicle_model.quat
pos_veh_delay1=self.pos_veh_prev1
pos_veh_delay2=self.pos_veh_prev2
pos_veh_delay3=self.pos_veh_prev3
pos_veh_delay4=self.pos_veh_prev4
pos_veh_delay5=self.pos_veh_prev5
pos_veh_delay6=self.pos_veh_prev6
self.pos_veh_prev1=self._vehicle_model.pos
self.pos_veh_prev2=pos_veh_delay1
self.pos_veh_prev3=pos_veh_delay2
self.pos_veh_prev4=pos_veh_delay3
self.pos_veh_prev5=pos_veh_delay4
self.pos_veh_prev6=pos_veh_delay5
self.pos_veh_prev7=pos_veh_delay6
vel_veh_delay1=self.vel_veh_prev1
vel_veh_delay2=self.vel_veh_prev2
vel_veh_delay3=self.vel_veh_prev3
vel_veh_delay4=self.vel_veh_prev4
vel_veh_delay5=self.vel_veh_prev5
vel_veh_delay6=self.vel_veh_prev6
self.vel_veh_prev1=self._vehicle_model.vel
self.vel_veh_prev2=vel_veh_delay1
self.vel_veh_prev3=vel_veh_delay2
self.vel_veh_prev4=vel_veh_delay3
self.vel_veh_prev5=vel_veh_delay4
self.vel_veh_prev6=vel_veh_delay5
self.vel_veh_prev7=vel_veh_delay6
quat_veh_delay1=self.quat_veh_prev1
quat_veh_delay2=self.quat_veh_prev2
quat_veh_delay3=self.quat_veh_prev3
quat_veh_delay4=self.quat_veh_prev4
quat_veh_delay5=self.quat_veh_prev5
quat_veh_delay6=self.quat_veh_prev6
self.quat_veh_prev1=self._vehicle_model.quat
self.quat_veh_prev2=quat_veh_delay1
self.quat_veh_prev3=quat_veh_delay2
self.quat_veh_prev4=quat_veh_delay3
self.quat_veh_prev5=quat_veh_delay4
self.quat_veh_prev6=quat_veh_delay5
self.quat_veh_prev7=quat_veh_delay6
t = rospy.get_time()
# non delay
self._errors['pos'] = np.dot(
rotItoB, self._reference['pos'] - pos)
self._errors['rot'] = quaternion_multiply(
quaternion_inverse(quat), self._reference['rot'])
self._errors['vel'] = np.hstack((
np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
# delay in reference
if t>40 and t<=100:
self._errors_ref_d['pos'] = np.dot(
rotItoB, self.pos_ref_prev7 - pos)
self._errors_ref_d['rot'] = quaternion_multiply(
quaternion_inverse(quat), self.rot_ref_prev7)
self._errors_ref_d['vel'] = np.hstack((
np.dot(rotItoB, self.vel_ref_prev7[0:3]) - vel[0:3],
np.dot(rotItoB, self.vel_ref_prev7[3:6]) - vel[3:6]))
else:
self._errors_ref_d['pos'] = np.dot(
rotItoB, self.pos_ref_prev1 - pos)
self._errors_ref_d['rot'] = quaternion_multiply(
quaternion_inverse(quat), self.rot_ref_prev1)
self._errors_ref_d['vel'] = np.hstack((
np.dot(rotItoB, self.vel_ref_prev1[0:3]) - vel[0:3],
np.dot(rotItoB, self.vel_ref_prev1[3:6]) - vel[3:6]))
# delay in vehicle measurement
if t > 40 and t<90:
self._errors_veh_d['pos'] = np.dot(rotItoB, self._reference['pos'] - self.pos_veh_prev4)
self._errors_veh_d['rot'] = quaternion_multiply(quaternion_inverse(self.quat_veh_prev4), self._reference['rot'])
self._errors_veh_d['vel'] = np.hstack((np.dot(rotItoB, self._reference['vel'][0:3]) - self.vel_veh_prev4[0:3],np.dot(rotItoB, self._reference['vel'][3:6]) - self.vel_veh_prev4[3:6]))
else:
self._errors_veh_d['pos'] = np.dot(rotItoB, self._reference['pos'] - self.pos_veh_prev2)
self._errors_veh_d['rot'] = quaternion_multiply(quaternion_inverse(self.quat_veh_prev2), self._reference['rot'])
self._errors_veh_d['vel'] = np.hstack((np.dot(rotItoB, self._reference['vel'][0:3]) - self.vel_veh_prev2[0:3],np.dot(rotItoB, self._reference['vel'][3:6]) - self.vel_veh_prev2[3:6]))
if self._error_pub.get_num_connections() > 0:
stamp = rospy.Time.now()
msg = TrajectoryPoint()
msg.header.stamp = stamp
msg.header.frame_id = self._local_planner.inertial_frame_id
# Publish pose error
msg.pose.position = Vector3(*np.dot(rotBtoI, self._errors['pos']))
msg.pose.orientation = Quaternion(*self._errors['rot'])
# Publish velocity errors in INERTIAL frame
msg.velocity.linear = Vector3(*np.dot(rotBtoI, self._errors['vel'][0:3]))
msg.velocity.angular = Vector3(*np.dot(rotBtoI, self._errors['vel'][3:6]))
self._error_pub.publish(msg)
def _update_reference(self):
"""Call the local planner interpolator to retrieve a trajectory
point and publish the reference message as `uuv_control_msgs/TrajectoryPoint`.
"""
# Update the local planner's information about the vehicle's pose
self._local_planner.update_vehicle_pose(
self._vehicle_model.pos, self._vehicle_model.quat)
t = rospy.get_time()
reference = self._local_planner.interpolate(t)
if reference is not None:
self._reference['pos'] = reference.p
self._reference['rot'] = reference.q
self._reference['vel'] = np.hstack((reference.v, reference.w))
self._reference['acc'] = np.hstack((reference.a, reference.alpha))
pos_ref_delay1=self.pos_ref_prev1
pos_ref_delay2=self.pos_ref_prev2
pos_ref_delay3=self.pos_ref_prev3
pos_ref_delay4=self.pos_ref_prev4
pos_ref_delay5=self.pos_ref_prev5
pos_ref_delay6=self.pos_ref_prev6
self.pos_ref_prev1=self._reference['pos']
self.pos_ref_prev2=pos_ref_delay1
self.pos_ref_prev3=pos_ref_delay2
self.pos_ref_prev4=pos_ref_delay3
self.pos_ref_prev5=pos_ref_delay4
self.pos_ref_prev6=pos_ref_delay5
self.pos_ref_prev7=pos_ref_delay6
rot_ref_delay1=self.rot_ref_prev1
rot_ref_delay2=self.rot_ref_prev2
rot_ref_delay3=self.rot_ref_prev3
rot_ref_delay4=self.rot_ref_prev4
rot_ref_delay5=self.rot_ref_prev5
rot_ref_delay6=self.rot_ref_prev6
self.rot_ref_prev1=self._reference['rot']
self.rot_ref_prev2=rot_ref_delay1
self.rot_ref_prev3=rot_ref_delay2
self.rot_ref_prev4=rot_ref_delay3
self.rot_ref_prev5=rot_ref_delay4
self.rot_ref_prev6=rot_ref_delay5
self.rot_ref_prev7=rot_ref_delay6
vel_ref_delay1=self.vel_ref_prev1
vel_ref_delay2=self.vel_ref_prev2
vel_ref_delay3=self.vel_ref_prev3
vel_ref_delay4=self.vel_ref_prev4
vel_ref_delay5=self.vel_ref_prev5
vel_ref_delay6=self.vel_ref_prev6
self.vel_ref_prev1=self._reference['vel']
self.vel_ref_prev2=vel_ref_delay1
self.vel_ref_prev3=vel_ref_delay2
self.vel_ref_prev4=vel_ref_delay3
self.vel_ref_prev5=vel_ref_delay4
self.vel_ref_prev6=vel_ref_delay5
self.vel_ref_prev7=vel_ref_delay6
acc_ref_delay1=self.acc_ref_prev1
acc_ref_delay2=self.acc_ref_prev2
acc_ref_delay3=self.acc_ref_prev3
acc_ref_delay4=self.acc_ref_prev4
acc_ref_delay5=self.acc_ref_prev5
acc_ref_delay6=self.acc_ref_prev6
self.acc_ref_prev1=self._reference['acc']
self.acc_ref_prev2=acc_ref_delay1
self.acc_ref_prev3=acc_ref_delay2
self.acc_ref_prev4=acc_ref_delay3
self.acc_ref_prev5=acc_ref_delay4
self.acc_ref_prev6=acc_ref_delay5
self.acc_ref_prev7=acc_ref_delay6
if reference is not None and self._reference_pub.get_num_connections() > 0:
# Publish current reference
msg = TrajectoryPoint()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self._local_planner.inertial_frame_id
msg.pose.position = Vector3(*self._reference['pos'])
msg.pose.orientation = Quaternion(*self._reference['rot'])
msg.velocity.linear = Vector3(*self._reference['vel'][0:3])
msg.velocity.angular = Vector3(*self._reference['vel'][3:6])
msg.acceleration.linear = Vector3(*self._reference['acc'][0:3])
msg.acceleration.angular = Vector3(*self._reference['acc'][3:6])
self._reference_pub.publish(msg)
return True