def joy_callback(self, msg):
"""Handle callbacks with joystick state."""
if not self.ready:
return
thrust = msg.axes[self.config['axis_thruster']] * \
self.config['scale_thruster']
rpy = numpy.array([
msg.axes[self.config['axis_roll']],
msg.axes[self.config['axis_pitch']],
msg.axes[self.config['axis_yaw']]
])
fins = self.rpy_to_fins.dot(rpy)
cmd = FloatStamped()
cmd.header.stamp = rospy.Time.now()
cmd.data = thrust
self.pub_thrust.publish(cmd)
for i in range(self.n_fins):
cmd.data = fins[i]
self.pub_cmd[i].publish(cmd)
if not self.ready:
return
def cmd_drive_callback(drive):
global p_left
global p_right
send = FloatStamped()
send.header.stamp = rospy.Time.now()
send.data = drive.left
p_left.publish(send)
send.data = drive.right
p_right.publish(send)
def callback(cmd):
global thruster_left
global thruster_right
send = FloatStamped()
send.header.stamp = rospy.Time.now()
send.data = cmd.left
thruster_left.publish(send)
send.data = cmd.right
thruster_right.publish(send)
def joy_callback(self, msg):
"""Handle callbacks with joystick state."""
if not self._ready:
return
try:
thrust = max(0, msg.axes[self._joy_axis['axis_thruster']]) * \
self._thruster_params['max_thrust'] * \
self._thruster_joy_gain
rpy = numpy.array([msg.axes[self._joy_axis['axis_roll']],
msg.axes[self._joy_axis['axis_pitch']],
msg.axes[self._joy_axis['axis_yaw']]])
fins = self._rpy_to_fins.dot(rpy)
self._thruster_model.publish_command(thrust)
for i in range(self._n_fins):
cmd = FloatStamped()
cmd.data = fins[i]
self._pub_cmd[i].publish(cmd)
if not self._ready:
return
except Exception, e:
print 'Error occurred while parsing joystick input, check if the joy_id corresponds to the joystick ' \
'being used. message=%s' % str(e)
def joy_callback(self, msg):
"""Handle callbacks with joystick state."""
if not self._ready:
return
try:
thrust = msg.axes[self._joy_axis['axis_thruster']] * \
self._thruster_params['max_thrust'] * \
self._thruster_joy_gain
rpy = numpy.array([
msg.axes[self._joy_axis['axis_roll']],
msg.axes[self._joy_axis['axis_pitch']],
msg.axes[self._joy_axis['axis_yaw']]
])
fins = self._rpy_to_fins.dot(rpy)
self._thruster_model.publish_command(thrust)
for i in range(self._n_fins):
cmd = FloatStamped()
cmd.data = fins[i]
self._pub_cmd[i].publish(cmd)
if not self._ready:
return
except Exception, e:
print 'Error occurred while parsing joystick input, check if the joy_id corresponds to the joystick ' \
'being used. message=%s' % str(e)
def pitch(self, sig): # -1 to 1, up to down
out = FloatStamped()
out.data = -60 * sig
self.bfin.publish(out)
self.fin5.publish(out)
out.data *= -1
self.fin4.publish(out)
def joy_callback(self, msg):
"""Handle callbacks with joystick state."""
if not self._ready:
return
thrust = msg.axes[self._joy_axis['axis_thruster']] * \
self._thruster_params['max_thrust']
rpy = numpy.array([
msg.axes[self._joy_axis['axis_roll']],
msg.axes[self._joy_axis['axis_pitch']],
msg.axes[self._joy_axis['axis_yaw']]
])
fins = self._rpy_to_fins.dot(rpy)
self._thruster_model.publish_command(thrust)
for i in range(self._n_fins):
cmd = FloatStamped()
cmd.data = fins[i]
self._pub_cmd[i].publish(cmd)
if not self._ready:
return
def yaw(self, sig): # -1 to 1, left to right
out = FloatStamped()
out.data = 60 * sig
self.fin0.publish(out)
self.fin1.publish(out)
out.data *= -1
self.fin2.publish(out)
self.fin3.publish(out)
def callback_esc(
msg
): # receive thruster input from controller and republish it to Gazebo
msgstamped = FloatStamped()
msgstamped.header.stamp = rospy.Time.now()
msgstamped.data = float(
msg.data
) * 100 # amplify the linear speed for Gazebo (value of 100 hundred needed to actually move forward)
pub_thruster.publish(msgstamped)
def pitch(self, direction, frame_id='lolo_auv_1_map'):
"""
+ = move up
"""
# direction = np.sign(direction)
out = FloatStamped()
out.header.frame_id = frame_id
out.data = -1 * direction
self.backfinspub.publish(out)
def yaw(self, direction, frame_id='lolo_auv_1_map'):
"""
+ = move left
"""
# direction = np.sign(direction)
out = FloatStamped()
out.header.frame_id = frame_id
out.data = 1 * direction
self.fin0pub.publish(out)
self.fin1pub.publish(out)
# the control for these fins are inverted for some reason
out = FloatStamped()
out.header.frame_id = frame_id
out.data = direction * -1
self.fin2pub.publish(out)
self.fin3pub.publish(out)
def update_controller(self):
if not self._is_init:
self._logger.info('Controller not initialized yet')
return False
world_pos_error = self._vehicle_model.rotBtoI.dot(self._errors['pos'])
world_rpy = euler_from_quaternion(self._vehicle_model.quat,
axes='sxyz')
thrust_control = self._thrust_gain * np.linalg.norm(
self._errors['pos'])
# Check for saturation
thrust_control = min(self._thruster_params['max_thrust'],
thrust_control)
thrust_control = -1 * max(self._min_thrust, thrust_control)
# Not using the custom wrench publisher that is configured per default
# for thruster-actuated vehicles
# Based on position tracking error: Compute desired orientation
pitch_des = -1 * np.arctan2(world_pos_error[2],
np.linalg.norm(world_pos_error[0:2]))
# Limit desired pitch angle:
pitch_err = self.unwrap_angle(pitch_des - world_rpy[1])
yaw_des = np.arctan2(world_pos_error[1], world_pos_error[0])
yaw_err = self.unwrap_angle(yaw_des - world_rpy[2])
# Limit yaw effort
yaw_err = min(np.pi / 4, max(-np.pi / 4, yaw_err))
rpy_control = np.array([
self._roll_gain * world_rpy[0], self._pitch_gain * pitch_err,
self._yaw_gain * yaw_err
])
fins_command = self._rpy_to_fins.dot(rpy_control)
# Check for saturation
for i in range(fins_command.size):
if np.abs(fins_command[i]) > self._max_fin_angle:
fins_command[i] = np.sign(
fins_command[i]) * self._max_fin_angle
# Publishing the commands to thruster and fins
self._thruster_model.publish_command(thrust_control)
cmd = FloatStamped()
cmd.header.stamp = rospy.Time.now()
for i in range(self._n_fins):
cmd.data = fins_command[i]
self._fin_pubs[i].publish(cmd)
return True
def test_input_output_topics_exist(self):
pub = rospy.Publisher('/vehicle/thrusters/0/input', FloatStamped,
queue_size=1)
for k in self.thruster_input_pub:
# Publishing set point to rotor velocity
input_message = FloatStamped()
input_message.header.stamp = rospy.Time.now()
input_message.data = 0.2
self.thruster_input_pub[k].publish(input_message)
sleep(1)
output = rospy.wait_for_message('/vehicle/thrusters/%d/thrust' % k,
FloatStamped, timeout=30)
self.assertIsNot(output.data, 0.0)
# Turning thruster off
input_message.data = 0.0
pub.publish(input_message)
def command_cb(self, msg):
"""Convert thrust to angular velocity commands for uuv_thruster_ros_plugin"""
for i in range(len(self._thrusters)):
cmd = FloatStamped()
force = eval("msg.force."+self._thrusters[i])
# Compute angular velocity from force
sign = 1 if force >= 0 else -1
ang_vel = sign * math.sqrt(abs(force / self._coeff))
cmd.header = msg.header
cmd.data = ang_vel
self._pubs[i].publish(cmd)
def test_input_output_topics_exist(self):
pub = rospy.Publisher('/vehicle/thrusters/0/input',
FloatStamped,
queue_size=1)
for k in self.thruster_input_pub:
# Publishing set point to rotor velocity
input_message = FloatStamped()
input_message.header.stamp = rospy.Time.now()
input_message.data = 0.2
self.thruster_input_pub[k].publish(input_message)
sleep(1)
output = rospy.wait_for_message('/vehicle/thrusters/%d/thrust' % k,
FloatStamped,
timeout=30)
self.assertIsNot(output.data, 0.0)
# Turning thruster off
input_message.data = 0.0
pub.publish(input_message)
def publish_command(self, thrust):
"""Publish the thrust force command set-point
to a thruster unit.
> *Input arguments*
* `thrust` (*type:* `float`): Thrust force set-point
in N.
"""
self._update(thrust)
output = FloatStamped()
output.header.stamp = self.node.get_clock().now().to_msg()
output.data = self._command
self._command_pub.publish(output)
def update_controller(self):
if not self._is_init:
self._logger.info('Controller not initialized yet')
return False
world_pos_error = self._vehicle_model.rotBtoI.dot(self._errors['pos'])
world_rpy = euler_from_quaternion(self._vehicle_model.quat, axes='sxyz')
thrust_control = self._thrust_gain * np.linalg.norm(self._errors['pos'])
# Check for saturation
thrust_control = min(self._thruster_params['max_thrust'], thrust_control)
thrust_control = -1 * max(self._min_thrust, thrust_control)
# Not using the custom wrench publisher that is configured per default
# for thruster-actuated vehicles
# Based on position tracking error: Compute desired orientation
pitch_des = -1 * np.arctan2(world_pos_error[2],
np.linalg.norm(world_pos_error[0:2]))
# Limit desired pitch angle:
pitch_err = self.unwrap_angle(pitch_des - world_rpy[1])
yaw_des = np.arctan2(world_pos_error[1], world_pos_error[0])
yaw_err = self.unwrap_angle(yaw_des - world_rpy[2])
# Limit yaw effort
yaw_err = min(np.pi / 4, max(-np.pi / 4, yaw_err))
rpy_control = np.array([self._roll_gain * world_rpy[0],
self._pitch_gain * pitch_err,
self._yaw_gain * yaw_err])
fins_command = self._rpy_to_fins.dot(rpy_control)
# Check for saturation
for i in range(fins_command.size):
if np.abs(fins_command[i]) > self._max_fin_angle:
fins_command[i] = np.sign(fins_command[i]) * self._max_fin_angle
# Publishing the commands to thruster and fins
self._thruster_model.publish_command(thrust_control)
cmd = FloatStamped()
cmd.header.stamp = rospy.Time.now()
for i in range(self._n_fins):
cmd.data = fins_command[i]
self._fin_pubs[i].publish(cmd)
return True
def set_thruster_rpm(self, thruster_rpms, time_sleep):
"""
It will set the speed of each thruster of the deepleng.
"""
x_thruster_rpm = FloatStamped()
y_thruster_rear_rpm = FloatStamped()
y_thruster_front_rpm = FloatStamped()
# diving_cell_rear_rpm = FloatStamped()
# diving_cell_front_rpm = FloatStamped()
x_thruster_rpm.data = thruster_rpms[0]
y_thruster_rear_rpm.data = thruster_rpms[1]
y_thruster_front_rpm.data = thruster_rpms[2]
# diving_cell_rear_rpm.data = thruster_rpms[3]
# diving_cell_front_rpm.data = thruster_rpms[4]
self.x_thruster.publish(x_thruster_rpm)
self.y_thruster_rear.publish(y_thruster_rear_rpm)
self.y_thruster_front.publish(y_thruster_front_rpm)
# self.diving_cell_front.publish(diving_cell_front_rpm)
# self.diving_cell_rear.publish(diving_cell_rear_rpm)
self.wait_time_for_execute_movement(time_sleep)
def joy_callback(self, msg):
"""Handle callbacks with joystick state."""
if not self._ready:
return
try:
thrust = max(0, msg.axes[self._joy_axis['axis_thruster']]) * \
self._thruster_params['max_thrust'].value * \
self._thruster_joy_gain
cmd_roll = msg.axes[self._joy_axis['axis_roll']]
if abs(cmd_roll) < 0.2:
cmd_roll = 0.0
cmd_pitch = msg.axes[self._joy_axis['axis_pitch']]
if abs(cmd_pitch) < 0.2:
cmd_pitch = 0.0
cmd_yaw = msg.axes[self._joy_axis['axis_yaw']]
if abs(cmd_yaw) < 0.2:
cmd_yaw = 0.0
rpy = numpy.array([cmd_roll, cmd_pitch, cmd_yaw])
fins = self._rpy_to_fins.dot(rpy)
self._thruster_model.publish_command(thrust)
for i in range(self._n_fins):
cmd = FloatStamped()
cmd.data = fins[i]
self._pub_cmd[i].publish(cmd)
if not self._ready:
return
except Exception as e:
self.get_logger().error('Error occurred while parsing joystick input, check '
'if the joy_id corresponds to the joystick '
'being used. message={}'.format(e))
def publish_command(self, thrust):
self._update(thrust)
output = FloatStamped()
output.header.stamp = rospy.Time.now()
output.data = self._command
self._command_pub.publish(output)
def publish_command(self, delta):
msg = FloatStamped()
msg.header.stamp = self.node.get_clock().now()#rospy.Time.now()
msg.data = delta
self.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 = 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 callback_servo1(
msg): # receive fin0 input from controller and republish it to Gazebo
msgstamped = FloatStamped()
msgstamped.header.stamp = rospy.Time.now()
msgstamped.data = float(msg.data)
pub_fin0.publish(msgstamped)
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)
for i in range(numberOfThrusters):
thrusterName = "rov/thrusters/" + str(i + 1) + "/input"
thrusterPubs.append(
rospy.Publisher(thrusterName, FloatStamped, queue_size=1))
rospy.loginfo("Created thruster publishers: " + str(thrusterPubs))
rate = rospy.Rate(50) #50Hz update frequency
while not rospy.is_shutdown():
#Update the thrusters by publishing the thrusterVals
for i in range(numberOfThrusters):
msg = FloatStamped()
#Add noise if the thruster value is not zero
if not thrusterVals[i] == 0:
#thrusterVals[i] += 2*random.random()-1 #random float between -1 to 1 exclusive
msg.data = thrusterVals[i]
else:
msg.data = thrusterVals[i]
thrusterPubs[i].publish(msg)
rospy.logdebug("Thrusters values published: " + str(thrusterVals))
#publish sensor hat data
tempMsg = Temperature()
#functions to give realistic sensor data curves over time
timeNowMin = rospy.get_time() / 60 ##Current ROS time in minutes
tempMsg.temperature = (
(60 * timeNowMin + 30) /
(timeNowMin + 1) - 2) + (0.5 * random.random() - 0.25)
humidMsg = RelativeHumidity()
humidMsg.relative_humidity = (
def key_teleop():
pub_t0_eff = rospy.Publisher('/leviathan/thrusters/0/input',
FloatStamped,
queue_size=10)
pub_t1_eff = rospy.Publisher('/leviathan/thrusters/1/input',
FloatStamped,
queue_size=10)
pub_t2_eff = rospy.Publisher('/leviathan/thrusters/2/input',
FloatStamped,
queue_size=10)
pub_t3_eff = rospy.Publisher('/leviathan/thrusters/3/input',
FloatStamped,
queue_size=10)
pub_t4_eff = rospy.Publisher('/leviathan/thrusters/4/input',
FloatStamped,
queue_size=10)
pub_t5_eff = rospy.Publisher('/leviathan/thrusters/5/input',
FloatStamped,
queue_size=10)
pub_t6_eff = rospy.Publisher('/leviathan/thrusters/6/input',
FloatStamped,
queue_size=10)
pub_t7_eff = rospy.Publisher('/leviathan/thrusters/7/input',
FloatStamped,
queue_size=10)
rospy.init_node('key_teleop', anonymous=True)
#rospy.Subscriber("/joy", Joy, joystick_state)
rate = rospy.Rate(30)
while not rospy.is_shutdown():
thrust0 = FloatStamped()
thrust0.header.stamp = rospy.Time.now()
thrust0.data = 10
pub_t0_eff.publish(thrust0)
thrust1 = FloatStamped()
thrust1.header.stamp = rospy.Time.now()
thrust1.data = 10
pub_t1_eff.publish(thrust1)
thrust2 = FloatStamped()
thrust2.header.stamp = rospy.Time.now()
thrust2.data = 10
pub_t2_eff.publish(thrust2)
thrust3 = FloatStamped()
thrust3.header.stamp = rospy.Time.now()
thrust3.data = 10
pub_t3_eff.publish(thrust3)
thrust4 = FloatStamped()
thrust4.header.stamp = rospy.Time.now()
thrust4.data = 10
pub_t4_eff.publish(thrust4)
thrust5 = FloatStamped()
thrust5.header.stamp = rospy.Time.now()
thrust5.data = 10
pub_t5_eff.publish(thrust5)
thrust6 = FloatStamped()
thrust6.header.stamp = rospy.Time.now()
thrust6.data = 10
pub_t6_eff.publish(thrust6)
thrust7 = FloatStamped()
thrust7.header.stamp = rospy.Time.now()
thrust7.data = 10
pub_t7_eff.publish(thrust7)
rate.sleep()
#!/usr/bin/env python
import numpy as np
import rospy
from uuv_gazebo_ros_plugins_msgs.msg import FloatStamped
if __name__ == "__main__":
rospy.init_node('thruster_pub', anonymous=True)
pub0 = rospy.Publisher('/bffv1/thrusters/0/input',
FloatStamped,
queue_size=1,
latch=True)
pub1 = rospy.Publisher('/bffv1/thrusters/1/input',
FloatStamped,
queue_size=1,
latch=True)
r = rospy.Rate(10)
val = rospy.get_param("~cmd", 500.0)
while not rospy.is_shutdown():
msg = FloatStamped()
msg.header.stamp = rospy.Time.now()
msg.data = val
msg0 = msg
msg1 = msg
msg1.data = msg0.data
msg0.header.frame_id = '/bffv1/thruster_0'
msg1.header.frame_id = '/bffv1/thruster_1'
pub0.publish(msg0)
pub1.publish(msg1)
r.sleep()
def publish_command(self, delta):
msg = FloatStamped()
msg.header.stamp = rospy.Time.now()
msg.data = delta
self.pub.publish(msg)
def thrust(self, sig): # 0 to 1
out = FloatStamped()
out.data = sig * 200
self.lthrust.publish(out)
self.rthrust.publish(out)
def publish_command(self, thrust, stamp):
self._update(thrust)
output = FloatStamped()
output.header.stamp = stamp
output.data = self._command
self._command_pub.publish(output)
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)
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 = 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))
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 = rospy.Time.now()
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 __init__(self, auv_name, with_ff, csv_fn, date):
# initialize
print('<auvOutlineControl>: initialize')
self.thruster0_pub_ = rospy.Publisher(auv_name + '/thrusters/0/input', FloatStamped, queue_size = 1)
self.fin0_pub_ = rospy.Publisher(auv_name + '/fins/0/input', FloatStamped, queue_size = 1)
self.fin1_pub_ = rospy.Publisher(auv_name + '/fins/1/input', FloatStamped, queue_size = 1)
self.fin2_pub_ = rospy.Publisher(auv_name + '/fins/2/input', FloatStamped, queue_size = 1)
self.fin3_pub_ = rospy.Publisher(auv_name + '/fins/3/input', FloatStamped, queue_size = 1)
self.outline_status_pub_ = rospy.Publisher(auv_name + '/outline_status', AUVOutlineStatus, queue_size = 1)
# parse
print('<auvOutlineControl>: parse csv')
self.auv_csv_parser_ = auv324CSVParser(date, with_ff)
self.auv_csv_parser_.parse_csv(csv_fn)
self.param_list_ = self.auv_csv_parser_.get_params()
rate = rospy.Rate(2)
# publish
print('<auvOutlineControl>: publish parsed parameters')
for i in range(len(self.param_list_)):
if not(rospy.is_shutdown()):
# thruster
th0_cmd = FloatStamped()
th0_cmd.data = self.param_list_[i].rpm_
self.thruster0_pub_.publish(th0_cmd)
# fin0
fin0_cmd = FloatStamped()
fin0_cmd.data = self.param_list_[i].fin0_
self.fin0_pub_.publish(fin0_cmd)
# fin1
fin1_cmd = FloatStamped()
fin1_cmd.data = self.param_list_[i].fin1_
self.fin1_pub_.publish(fin1_cmd)
# fin2
fin2_cmd = FloatStamped()
fin2_cmd.data = self.param_list_[i].fin2_
self.fin2_pub_.publish(fin2_cmd)
# fin3
fin3_cmd = FloatStamped()
fin3_cmd.data = self.param_list_[i].fin3_
self.fin3_pub_.publish(fin3_cmd)
outline_status_msg = AUVOutlineStatus()
outline_status_msg.header.frame_id = auv_name + '/base_link'
outline_status_msg.x = self.param_list_[i].x_
outline_status_msg.y = self.param_list_[i].y_
outline_status_msg.z = self.param_list_[i].z_
outline_status_msg.roll = self.param_list_[i].roll_
outline_status_msg.pitch = self.param_list_[i].pitch_
outline_status_msg.yaw = self.param_list_[i].yaw_
outline_status_msg.u = self.param_list_[i].u_
outline_status_msg.v = self.param_list_[i].v_
outline_status_msg.w = self.param_list_[i].w_
outline_status_msg.p = self.param_list_[i].p_
outline_status_msg.q = self.param_list_[i].q_
outline_status_msg.r = self.param_list_[i].r_
outline_status_msg.ts = self.param_list_[i].ts_
outline_status_msg.fin0 = self.param_list_[i].fin0_
outline_status_msg.fin1 = self.param_list_[i].fin1_
outline_status_msg.fin2 = self.param_list_[i].fin2_
outline_status_msg.fin3 = self.param_list_[i].fin3_
self.outline_status_pub_.publish(outline_status_msg)
rate.sleep()
def __init__(self, auv_name, db_fn, table, date):
# initialize
print('<auvOutlineControl>: initialize')
self.thruster0_pub_ = rospy.Publisher(auv_name + '/thruster/0/input',
FloatStamped,
queue_size=1)
self.fin0_pub_ = rospy.Publisher(auv_name + '/fins/0/input',
FloatStamped,
queue_size=1)
self.fin1_pub_ = rospy.Publisher(auv_name + '/fins/1/input',
FloatStamped,
queue_size=1)
self.fin2_pub_ = rospy.Publisher(auv_name + '/fins/2/input',
FloatStamped,
queue_size=1)
self.fin3_pub_ = rospy.Publisher(auv_name + '/fins/3/input',
FloatStamped,
queue_size=1)
self.fin4_pub_ = rospy.Publisher(auv_name + '/fins/4/input',
FloatStamped,
queue_size=1)
self.fin5_pub_ = rospy.Publisher(auv_name + '/fins/5/input',
FloatStamped,
queue_size=1)
# parse
print('<auvOutlineControl>: parse database')
self.auv_db_parser_ = auvDBParser(table, date)
self.auv_db_parser_.parse_db(db_fn)
self.param_list_ = self.auv_db_parser_.get_params()
rate = rospy.Rate(10) # 10hz
# publish
print('<auvOutlineControl>: publish parsed parameters')
for i in range(len(self.param_list_)):
if not (rospy.is_shutdown()):
# thruster
th0_cmd = FloatStamped()
th0_cmd.data = self.param_list_[i].rpm_
self.thruster0_pub_.publish(th0_cmd)
# fin0
fin0_cmd = FloatStamped()
fin0_cmd.data = self.param_list_[i].fin0_
self.fin0_pub_.publish(fin0_cmd)
# fin1
fin1_cmd = FloatStamped()
fin1_cmd.data = self.param_list_[i].fin1_
self.fin1_pub_.publish(fin1_cmd)
# fin2
fin2_cmd = FloatStamped()
fin2_cmd.data = self.param_list_[i].fin1_
self.fin2_pub_.publish(fin2_cmd)
# fin3
fin3_cmd = FloatStamped()
fin3_cmd.data = self.param_list_[i].fin1_
self.fin3_pub_.publish(fin3_cmd)
# fin4
fin4_cmd = FloatStamped()
fin4_cmd.data = self.param_list_[i].fin1_
self.fin4_pub_.publish(fin4_cmd)
# fin5
fin5_cmd = FloatStamped()
fin5_cmd.data = self.param_list_[i].fin1_
self.fin5_pub_.publish(fin5_cmd)
rate.sleep()
def publish_command(self, thrust):
self._update(thrust)
output = FloatStamped()
output.header.stamp = rospy.Time.now()
output.data = self._command
self._command_pub.publish(output)
def joy_callback(self, joy):
msg = FloatStamped()
for aa, ii in zip(joy.axes, range(len(joy.axes))):
if ii in self.joy2thrust.keys():
msg.data = aa * self.gain
self.joy2thrust[ii].publish(msg)
def publish_command(self, delta):
msg = FloatStamped()
msg.header.stamp = rospy.Time.now()
msg.data = delta
self.pub.publish(msg)