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 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 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.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 __init__(self):
super().__init__('torpedo_joy')
self.drive_val = 0.0
self.drive_f64 = FloatStamped()
self.drive_f64.data = 0.0
self.yaw_val = 0.0
self.yaw_f64 = FloatStamped()
self.yaw_f64.data = 0.0
self.pitch_val = 0.0
self.pitch_f64 = FloatStamped()
self.pitch_f64.data = 0.0
self.pub_yaw = self.create_publisher(FloatStamped,
'/babyyoda/fins/id_0/input', 10)
self.pub_pitch = self.create_publisher(FloatStamped,
'/babyyoda/fins/id_1/input', 10)
self.pub_drive = self.create_publisher(
FloatStamped, '/babyyoda/thrusters/id_0/input', 10)
self.subscription = self.create_subscription(Joy, 'joy',
self.joystick_state, 10)
self.subscription # prevent unused variable warning
timer_period = 1 / 30.0 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
self.i = 0
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 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 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 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 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 command_cb(self, msg):
"""Read in thruster commands from main vehicle, then convert to angular velocity commands for uuv_thruster_ros_plugin"""
for thruster in self._thrusters:
cmd = FloatStamped()
force = 0
code = "force = msg.force." + thruster
exec(code) # Run the command, get thruster force
ang_vel = self.get_angular_velocity(force)
cmd.header = msg.header
code = "cmd.data = " + str(ang_vel)
exec(code) # Run the command, populate message
self._pubs[thruster].publish(cmd)
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 publish_variables(self, event):
t = rospy.get_time()
for var in self._variables:
pc_msg = PointCloud()
pc_msg.header.stamp = rospy.Time.now()
pc_msg.header.frame_id = 'world'
for name in self._vehicle_pos:
value = self._interpolate(var, self._vehicle_pos[name][0],
self._vehicle_pos[name][1],
self._vehicle_pos[name][2], t)
# Updating the point cloud for this variable
pc_msg.points.append(
Point32(self._vehicle_pos[name][0],
self._vehicle_pos[name][1],
self._vehicle_pos[name][2]))
pc_msg.channels.append(ChannelFloat32())
pc_msg.channels[-1].name = 'intensity'
pc_msg.channels[-1].values.append(value)
# Create the message objects
if 'temperature' in var.lower():
msg = Temperature()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '%s/base_link' % name
# TODO Read from the unit of temperature from NC file
# Converting to Celsius
msg.temperature = value - 273.15
else:
msg = FloatStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '%s/base_link' % name
msg.data = value
self._topics[name][var].publish(msg)
self._pc_variables[var].publish(pc_msg)
return True
def motor_control(self):
"""Main loop manages updating motor thrusts"""
for i in xrange(8):
self.thrust.append(FloatStamped())
self.thrust[i].data = 0
rospy.init_node('gazebo_motor_control', anonymous=True)
rospy.Subscriber(
'cusub_common/motor_controllers/pololu_control/command',
Float64MultiArray, self.motor_command_callback)
pub_thrust = []
for i in xrange(8):
pub_thrust.append(rospy.Publisher('description/thrusters/' \
+ str(i) + '/input',
FloatStamped, queue_size=1))
rate = rospy.Rate(30)
while not rospy.is_shutdown():
for i in xrange(8):
pub_thrust[i].publish(self.thrust[i])
rate.sleep()
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 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 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 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 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 __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, delta):
msg = FloatStamped()
msg.header.stamp = self.node.get_clock().now()#rospy.Time.now()
msg.data = delta
self.pub.publish(msg)
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 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 = 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)
queue_size=1)
#create a list of thruster publishers
thrusterPubs = []
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 = (
def seadragonThrusterDepthBackRightCallback(self, msg):
self.gazeboThrusterDepthBackRightPub.publish(
FloatStamped(data=msg.data * self.PWM_SCALING))
def seadragonThrusterYawBackLeftCallback(self, msg):
self.gazeboThrusterYawBackLeftPub.publish(
FloatStamped(data=msg.data * self.PWM_SCALING * -1))
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 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 thrust(self, sig): # 0 to 1
out = FloatStamped()
out.data = sig * 200
self.lthrust.publish(out)
self.rthrust.publish(out)
def publish_command(self, delta):
msg = FloatStamped()
msg.header.stamp = rospy.Time.now()
msg.data = delta
self.pub.publish(msg)
