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 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 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)