def __init__(self, node_name, **kwargs): super().__init__(node_name, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True, **kwargs) # sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True) # self.set_parameters([sim_time]) self._logger = logging.getLogger('dp_local_planner') out_hdlr = logging.StreamHandler(sys.stdout) out_hdlr.setFormatter(logging.Formatter('%(asctime)s | %(levelname)s | %(module)s | %(message)s')) out_hdlr.setLevel(logging.INFO) self._logger.addHandler(out_hdlr) self._logger.setLevel(logging.INFO) self.thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True) self.thread.start() # Load disturbances and check for missing information specs = dict(current=['starting_time', 'velocity', 'horizontal_angle', 'vertical_angle'], wrench=['starting_time', 'duration', 'force', 'torque'], thruster_state=['starting_time', 'thruster_id', 'is_on', 'duration'], propeller_efficiency=['starting_time', 'thruster_id', 'duration', 'efficiency'], thrust_efficiency=['starting_time', 'thruster_id', 'duration', 'efficiency']) thruster_ids = list() #if self.has_parameter('disturbances'): #self._logger.info(str(self.get_parameters(['/']))) self._disturbances = self.get_parameters_by_prefix('disturbances') self._disturbances = parse_nested_params_to_dict(self._disturbances, unpack_value=True) if self._disturbances != {}: #if type(self._disturbances) != list: # raise RuntimeError('Current specifications must be ' # 'given as a list of dict') #for i in range(len(self._disturbances)): for key in self._disturbances.keys(): item = self._disturbances[key] if type(item) != dict: raise RuntimeError('Disturbance description must be' ' given as a dict') if 'type' not in item: raise RuntimeError('Type of disturbance not ' 'specified') if item['type'] not in specs: raise RuntimeError( 'Invalid type of disturbance, value=%s' % item['type']) for spec in specs[item['type']]: if spec not in item: raise RuntimeError( 'Invalid current model specification, ' 'missing tag=%s' % spec) if item['type'] == 'thruster_state': thruster_ids.append(item['thruster_id']) # Create flag to indicate that perturbation has been applied self._disturbances[key]['is_applied'] = False self._disturbances[key]['ended'] = False else: raise RuntimeError('No disturbance specifications given') # List all disturbances to be applied #for i in range(len(self._disturbances)): cpt = 0 for key in self._disturbances.keys(): self._logger.info('Disturbance #%d: %s' % (cpt, self._disturbances[key])) cpt = cpt + 1 self._body_force = np.zeros(3) self._body_torque = np.zeros(3) self._body_wrench_msg = WrenchStamped() # For body wrench disturbances, publish a topic self._wrench_topic = self.create_publisher( WrenchStamped, 'wrench_perturbation', 1) vehicle_name = self.get_namespace().replace('/', '') # Obtain service proxy self._service_cb = dict() service_list = list() try: service_list.append(self.create_client( SetCurrentVelocity, '/hydrodynamics/set_current_velocity')) self._service_cb['current_velocity'] = service_list[-1] service_list.append(self.create_client( ApplyBodyWrench, '/gazebo/apply_body_wrench')) self._service_cb['wrench'] = service_list[-1] self._service_cb['thrusters'] = dict() for item in self._disturbances.values(): if item['type'] == 'thruster_state': thruster_id = item['thruster_id'] if 'state' not in self._service_cb['thrusters']: self._service_cb['thrusters']['state'] = dict() service_list.append(self.create_client( SetThrusterState, create_service_name(vehicle_name, thruster_id, 'set_thruster_state'))) #'/%s/thrusters/%d/set_thruster_state' % (vehicle_name, item['thruster_id']))) self._service_cb['thrusters']['state'][thruster_id] = service_list[-1] elif item['type'] == 'propeller_efficiency': if 'propeller_efficiency' not in self._service_cb['thrusters']: self._service_cb['thrusters']['propeller_efficiency'] = dict() service_list.append(self.create_client( SetThrusterEfficiency, create_service_name(vehicle_name, thruster_id, 'set_dynamic_state_efficiency'))) #'/%s/thrusters/%d/set_dynamic_state_efficiency' % (vehicle_name, item['thruster_id']))) self._service_cb['thrusters']['propeller_efficiency'][thruster_id] = service_list[-1] elif item['type'] == 'thrust_efficiency': if 'thrust_efficiency' not in self._service_cb['thrusters']: self._service_cb['thrusters']['thrust_efficiency'] = dict() service_list.append(self.create_client( SetThrusterEfficiency, create_service_name(vehicle_name, thruster_id, 'set_thrust_force_efficiency'))) #'/%s/thrusters/%d/set_thrust_force_efficiency' % (vehicle_name, item['thruster_id']))) self._service_cb['thrusters']['thrust_efficiency'][thruster_id] = service_list[-1] except Exception as e: self._logger.info('Service call failed, error=%s' % str(e)) sys.exit(-1) # Test if services are reachable try: #self._service_cb.values() # services = ['/hydrodynamics/set_current_velocity', # '/gazebo/apply_body_wrench'] # for item in self._disturbances: # if item['type'] == 'thruster_state': # services.append('/%s/thrusters/%d/set_thruster_state' % (vehicle_name, item['thruster_id'])) # elif item['type'] == 'propeller_efficiency': # services.append('/%s/thrusters/%d/set_dynamic_state_efficiency' % (vehicle_name, item['thruster_id'])) # elif item['type'] == 'thrust_efficiency': # services.append('/%s/thrusters/%d/set_thrust_force_efficiency' % (vehicle_name, item['thruster_id'])) for s in service_list: ready = s.wait_for_service(timeout_sec=10) if not ready: raise RuntimeError('Service %s not ready' % (s.srv_name)) except Exception as e: self._logger.error('Some services are not available! message=' + str(e)) self._logger.error('Closing node...') sys.exit(-1) self._wrench_timer = self.create_timer(0.1, self._publish_wrench_disturbance) #rate = self.create_rate(100) #FREQ = 100 rate = node.create_rate(100) while rclpy.ok(): t = time_in_float_seconds(self.get_clock().now()) #for i in range(len(self._disturbances)): for d in self._disturbances.values(): #d = self._disturbances[i] if t > d['starting_time'] and not d['is_applied']: ########################################################### if d['type'] == 'current': self.set_current(d['velocity'], d['horizontal_angle'], d['vertical_angle']) ########################################################### elif d['type'] == 'wrench': self.set_link_wrench(d['force'], d['torque'], -1, d['starting_time']) ########################################################### elif d['type'] == 'thruster_state': self.set_thruster_state(d['thruster_id'], bool(d['is_on'])) ########################################################### elif d['type'] == 'propeller_efficiency': self.set_propeller_efficiency(d['thruster_id'], d['efficiency']) ########################################################### elif d['type'] == 'thrust_efficiency': self.set_thrust_efficiency(d['thruster_id'], d['efficiency']) # Set applied flag to true #self._disturbances[i]['is_applied'] = True d['is_applied'] = True if 'duration' in d: if d['duration'] == -1: #self._disturbances[i]['ended'] = True d['ended'] = True else: #self._disturbances[i]['ended'] = True d['ended'] = True elif d['is_applied'] and 'duration' in d and not d['ended']: if d['duration'] > 0: if self.get_clock().now().nanoseconds > int((d['starting_time'] + d['duration']) * 1e9): ########################################################### if d['type'] == 'current': # Set current to zero self.set_current(0, d['horizontal_angle'], d['vertical_angle']) ########################################################### elif d['type'] == 'wrench': # Cancel out force and torque self.set_link_wrench([-1 * d['force'][n] for n in range(3)], [-1 * d['torque'][n] for n in range(3)], -1, time_in_float_sec(self.get_clock().now())) ########################################################### elif d['type'] == 'thruster_state': self.set_thruster_state(d['thruster_id'], not bool(d['is_on'])) ########################################################### elif d['type'] == 'propeller_efficiency': self.set_propeller_efficiency(d['thruster_id'], 1.0) ########################################################### elif d['type'] == 'thrust_efficiency': self.set_thrust_efficiency(d['thruster_id'], 1.0) #self._disturbances[i]['ended'] = True d['ended'] = True rate.sleep()
def _update_time_step(self): """Update time step.""" t = time_in_float_sec(self.get_clock().now()) self._dt = t - self._prev_time self._prev_time = t
def main(): rclpy.init() sim_time_param = is_sim_time() node = rclpy.create_node( 'start_circular_trajectory', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True, parameter_overrides=[sim_time_param]) node.get_logger().info('Starting the circular trajectory creator') # Important...ensure the clock has been updated when using sim time while node.get_clock().now() == rclpy.time.Time( clock_type=node.get_clock().clock_type): rclpy.spin_once(node) # If no start time is provided: start *now*. start_time = time_in_float_sec(node.get_clock().now()) start_now = False if node.has_parameter('start_time'): start_time = node.get_parameter('start_time').value if start_time < 0.0: node.get_logger().warn('Negative start time, setting it to 0.0') start_time = 0.0 start_now = True else: start_now = True param_labels = [ 'radius', 'center', 'n_points', 'heading_offset', 'duration', 'max_forward_speed' ] params = dict() for label in param_labels: if not node.has_parameter(label): node.get_logger().error( '{} must be provided for the trajectory generation!'.format( label)) sys.exit(-1) params[label] = node.get_parameter(label).value if len(params['center']) != 3: node.get_logger().error( 'Center of circle must have 3 components (x, y, z): ' + str(params['center'])) sys.exit(-1) if params['n_points'] <= 2: node.get_logger().error('Number of points must be at least 2: ' + str(params['n_points'])) sys.exit(-1) if params['max_forward_speed'] <= 0: node.get_logger().error('Velocity limit must be positive: ' + str(params['max_forward_speed'])) sys.exit(-1) srv_name = 'start_circular_trajectory' traj_gen = node.create_client(InitCircularTrajectory, srv_name) if not traj_gen.wait_for_service(timeout_sec=20): node.get_logger().error('Service %s not available! Closing node...' % (traj_gen.srv_name)) sys.exit(-1) node.get_logger().info( 'Generating trajectory that starts at t={} s'.format(start_time)) # Convert the time value (sec, nsec) = float_sec_to_int_sec_nano(start_time) req = InitCircularTrajectory.Request() req.start_time = rclpy.time.Time(seconds=sec, nanoseconds=nsec).to_msg() req.start_now = start_now req.radius = params['radius'] req.center = Point(params['center'][0], params['center'][1], params['center'][2]) req.is_clockwise = False req.angle_offset = 0.0 req.n_points = params['n_points'] req.heading_offset = params['heading_offset'] * pi / 180 req.max_forward_speed = params['max_forward_speed'] req.duration = params['duration'] future = traj_gen.call_async(req) rclpy.spin_until_future_complete(node, future) try: response = future.result() except Exception as e: node.get_logger().error('Service call ' + srv_name + ' failed, error=' + str(e)) else: node.get_logger().info('Trajectory successfully generated!') node.destroy_node()
def update_controller(self): if not self._is_init: return False t = time_in_float_sec(self.get_clock().now()) dt = t - self._prev_t if self._prev_t < 0.0: dt = 0.0 # Get trajectory errors (reference - actual) e_p_linear_b = self._errors['pos'] e_v_linear_b = self._errors['vel'][0:3] e_p_angular_b = self.error_orientation_rpy # check this e_v_angular_b = self._errors['vel'][3:6] # Compute sliding surface s wrt body frame s_linear_b = -e_v_linear_b - np.multiply(self._slope[0:3], e_p_linear_b) s_angular_b = -e_v_angular_b - np.multiply(self._slope[3:6], e_p_angular_b) # Compute exponential decay for transient improvement if self._first_pass == True: self._t_init, self._s_linear_b_init, self._s_angular_b_init = t, s_linear_b, s_angular_b self._first_pass = False sd_linear_b = np.multiply(self._s_linear_b_init, np.exp(-self._K[0:3] * (t - self._t_init))) sd_angular_b = np.multiply(self._s_angular_b_init, np.exp(-self._K[3:6] * (t - self._t_init))) # Compute sliding surface sn wrt body frame sn_linear_b = s_linear_b - sd_linear_b sn_angular_b = s_angular_b - sd_angular_b # Compute summation sign(sn) wrt body frame if self._prev_t > 0.0 and dt > 0.0: self._summ_sign_sn_linear_b = self._summ_sign_sn_linear_b + 0.5 * ( self.sat(sn_linear_b, self._sat_epsilon) + self._prev_sign_sn_linear_b) * dt self._summ_sign_sn_angular_b = self._summ_sign_sn_angular_b + 0.5 * ( self.sat(sn_angular_b, self._sat_epsilon) + self._prev_sign_sn_angular_b) * dt # Compute extended error wrt body frame sr_linear_b = sn_linear_b + np.multiply(self._Ki[0:3], self._summ_sign_sn_linear_b) sr_angular_b = sn_angular_b + np.multiply(self._Ki[3:6], self._summ_sign_sn_angular_b) # Compute required forces and torques wrt body frame force_b = -np.multiply(self._Kd[0:3], sr_linear_b) torque_b = -np.multiply(self._Kd[3:6], sr_angular_b) self._tau = np.hstack((force_b, torque_b)) self.publish_control_wrench(self._tau) self._prev_sign_sn_linear_b = self.sat(sn_linear_b, self._sat_epsilon) self._prev_sign_sn_angular_b = self.sat(sn_angular_b, self._sat_epsilon) self._prev_t = t
def __init__(self, name, is_model_based=False, list_odometry_callbacks=None, planner_full_dof=False, world_ned_to_enu=None, **kwargs): super().__init__(name, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True, **kwargs) # Flag will be set to true when all parameters are initialized correctly self._is_init = False self._logger = get_logger() # Reading current namespace self._namespace = self.get_namespace() # Configuration for the vehicle dynamic model self._is_model_based = is_model_based if self._is_model_based: self._logger.info('Setting controller as model-based') else: self._logger.info('Setting controller as non-model-based') self._use_stamped_poses_only = False if self.has_parameter('use_stamped_poses_only'): self._use_stamped_poses_only = self.get_parameter('use_stamped_poses_only').get_parameter_value().bool_value # Flag indicating if the vehicle has only thrusters, otherwise # the AUV allocation node will be used self.thrusters_only = self.get_parameter_or_helper('thrusters_only', True).get_parameter_value().bool_value self._callback_group = rclpy.callback_groups.ReentrantCallbackGroup() # Instance of the local planner for local trajectory generation self._local_planner = LocalPlanner( self, full_dof=planner_full_dof, stamped_pose_only=self._use_stamped_poses_only, thrusters_only=self.thrusters_only, callback_group = self._callback_group, tf_trans_world_ned_to_enu=world_ned_to_enu) self._control_saturation = 5000 # TODO: Fix the saturation term and how it is applied if self.has_parameter('saturation'): self._thrust_saturation = self.get_parameter('saturation').value if self._control_saturation <= 0: raise RuntimeError('Invalid control saturation forces') # Flag indicating either use of the AUV control allocator or # direct command of fins and thruster self.use_auv_control_allocator = False if not self.thrusters_only: self.use_auv_control_allocator = self.get_parameter_or_helper( 'use_auv_control_allocator', False).get_parameter_value().bool_value # Remap the following topics, if needed # Publisher for thruster allocator if self.thrusters_only: self._thrust_pub = self.create_publisher( WrenchStamped, 'thruster_output', 1) else: self._thrust_pub = None if not self.thrusters_only: self._auv_command_pub = self.create_publisher( AUVCommand, 'auv_command_output', 1) else: self._auv_command_pub = None self._min_thrust = self.get_parameter_or_helper('min_thrust', 40.0).get_parameter_value().double_value self._reference_pub = self.create_publisher(TrajectoryPoint, 'reference', 1) # Publish error (for debugging) self._error_pub = self.create_publisher(TrajectoryPoint, 'error', 1) self._init_reference = False # Reference with relation to the INERTIAL frame self._reference = dict(pos=np.zeros(3), rot=np.zeros(4), vel=np.zeros(6), acc=np.zeros(6)) # Errors wih relation to the BODY frame self._errors = dict(pos=np.zeros(3), rot=np.zeros(4), vel=np.zeros(6)) # Time step self._dt = 0 self._prev_time = time_in_float_sec(self.get_clock().now()) self._services = dict() self._services['reset'] = self.create_service(ResetController, 'reset_controller', self.reset_controller_callback, callback_group=self._callback_group) # Time stamp for the received trajectory self._stamp_trajectory_received = time_in_float_sec(self.get_clock().now()) # Instance of the vehicle model self._vehicle_model = None # If list of callbacks is empty, set the default if list_odometry_callbacks is not None and \ isinstance(list_odometry_callbacks, list): self._odometry_callbacks = list_odometry_callbacks else: self._odometry_callbacks = [self.update_errors, self.update_controller] # Initialize vehicle, if model based self._create_vehicle_model(world_ned_to_enu) # Flag to indicate that odometry topic is receiving data self._init_odom = False # Subscribe to odometry topic self._odom_topic_sub = self.create_subscription( Odometry, 'odom', self._odometry_callback, 10, callback_group=self._callback_group) # Stores last simulation time self._prev_t = -1.0 self._logger.info('DP controller successfully initialized')
def main(): rclpy.init() sim_time_param = is_sim_time() node = rclpy.create_node( 'set_thrusters_output_efficiency', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True, parameter_overrides=[sim_time_param]) node.get_logger().info('Set the thruster output efficiency for vehicle, namespace=' + self.get_namespace()) # sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True) # node.set_parameters([sim_time]) starting_time = 0.0 if node.has_parameter('starting_time'): starting_time = float(node.get_parameter('starting_time').value) node.get_logger().info('Starting time= {} s'.format(starting_time)) duration = 0.0 if node.has_parameter('duration'): duration = float(node.get_parameter('duration').value) if duration <= 0.0: raise RuntimeError('Duration not set, leaving node...') node.get_logger().info('Duration [s]=', ('Inf.' if duration < 0 else str(duration))) if node.has_parameter('efficiency'): efficiency = float(node.get_parameter('efficiency').value) if efficiency < 0 or efficiency > 1: raise RuntimeError('Invalid thruster output efficiency, leaving node...') else: raise RuntimeError('Thruster output efficiency not set, leaving node...') thruster_id = -1 if node.has_parameter('thruster_id'): if node.get_parameter('thruster_id').type_ == rclpy.Parameter.Type.INTEGER: thruster_id = node.get_parameter('thruster_id').get_parameter_value().integer_value else: raise RuntimeError('Thruster ID not given') if thruster_id < 0: raise RuntimeError('Invalid thruster ID') node.get_logger().info('Setting thruster output efficiency #{} to {}'.format(thruster_id, 100 * efficiency)) vehicle_name = node.get_namespace().replace('/', '') srv_name = build_service_name(vehicle_name, thruster_id, 'set_thrust_force_efficiency') set_eff = node.create_client(SetThrusterEfficiency, srv_name) if not set_eff.wait_for_service(timeout_sec=2): raise RuntimeError('Service %s not available! Closing node...' %(srv_name)) FREQ = 100 rate = node.create_rate(FREQ) thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) thread.start() while time_in_float_sec(node.get_clock().now()) < starting_time: #Just a guard for really short timeouts if 1.0 / FREQ < starting_time: rate.sleep() # rate = node.create_rate(100) # while time_in_float_sec(node.get_clock().now()) < starting_time: # rate.sleep() req = SetThrusterEfficiency.Request() req.efficiency = efficiency #success = set_eff.call(efficiency) future = set_eff.call_async(efficiency) #NB : spining is done from another thread while not future.done(): try: response = future.result() except Exception as e: node.get_logger().info('Service call ' + srv_name + ' failed, error=' + str(e)): else: node.get_logger().info('Time={} s'.format(rtime_in_float_sec(node.get_clock().now()))) node.get_logger().info('Current thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100)) if duration > 0: while time_in_float_sec(node.get_clock().now()) < starting_time + duration: if 1.0 / FREQ < starting_time + duration: rate.sleep() # rate = node.create_rate.Rate(100) # while time_in_float_sec(node.get_clock().now()) < starting_time + duration: # rate.sleep() req.efficiency = 1.0 success = set_eff.call(efficiency) while not future.done(): try: response = future.result() except Exception as e: node.get_logger().info('Service call ' + srv_name + ' failed, error=' + str(e)): else: node.get_logger().info('Time={} s'.format(time_in_float_sec(node.get_clock().now()))) node.get_logger().infoint( 'Returning to previous thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100)) # if success: # print('Time={} s'.format(time_in_float_sec(node.get_clock().now()))) # print('Returning to previous thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100)) node.get_logger().info('Leaving node...') node.destroy_node() rclpy.shutdown() thread.join()
def update_controller(self): if not self._is_init: return False t = time_in_float_sec(self.get_clock().now()) dt = t - self._prev_t if self._prev_t < 0.0: dt = 0.0 # Update integrator self._int += 0.5 * (self.error_pose_euler - self._error_pose) * self._dt # Store current pose error self._error_pose = self.error_pose_euler # Get trajectory errors (reference - actual) e_p_linear_b = self._errors['pos'] e_v_linear_b = self._errors['vel'][0:3] e_p_angular_b = self.error_orientation_rpy e_v_angular_b = self._errors['vel'][3:6] e_p_b = np.hstack((e_p_linear_b, e_p_angular_b)) e_v_b = np.hstack((e_v_linear_b, e_v_angular_b)) # Compute sliding surface s wrt body frame self._s_b = -e_v_b - np.multiply(self._lambda, e_p_b) \ - self._sliding_int * np.multiply(np.square(self._lambda)/4, self._int) # Acceleration estimate self._rotBtoI_dot = np.dot( cross_product_operator(self._vehicle_model._vel[3:6]), self._vehicle_model.rotBtoI) self._accel_linear_estimate_b = np.dot( self._vehicle_model.rotItoB, (self._reference['acc'][0:3] - \ np.dot(self._rotBtoI_dot, self._vehicle_model._vel[0:3]))) + \ np.multiply(self._lambda[0:3], e_v_linear_b) + \ self._sliding_int * np.multiply(np.square(self._lambda[0:3]) / 4, e_p_linear_b) self._accel_angular_estimate_b = np.dot(self._vehicle_model.rotItoB, (self._reference['acc'][3:6] - np.dot(self._rotBtoI_dot, self._vehicle_model._vel[3:6]))) + \ np.multiply(self._lambda[3:6], e_v_angular_b) + \ self._sliding_int * np.multiply(np.square(self._lambda[3:6]) / 4, e_p_angular_b) self._accel_estimate_b = np.hstack( (self._accel_linear_estimate_b, self._accel_angular_estimate_b)) # Equivalent control acc = self._vehicle_model.to_SNAME(self._accel_estimate_b) self._f_eq = self._vehicle_model.compute_force(acc, use_sname=False) # Linear control self._f_lin = -np.multiply(self._k, self._s_b) # Uncertainties / disturbances upper boundaries for robust control self._rho_total = self._adaptive_bounds * self._rho_adapt + self._constant_bound * self._rho_constant # Adaptation law self._rho_adapt = self._rho_adapt + \ (self._adapt_slope[0] * np.abs(self._s_b) + (self._adapt_slope[1] * np.abs(self._s_b) * np.abs(e_p_b) * np.abs(e_p_b)) + (self._adapt_slope[2] * np.abs(self._s_b) * np.abs(e_v_b) * np.abs(e_v_b)) + self._drift_prevent * (self._rho_0 - self._rho_adapt)) * dt # Robust control self._f_robust = -np.multiply( self._rho_total, (2 / np.pi) * np.arctan(np.multiply(self._c, self._s_b))) # Compute required forces and torques wrt body frame self._tau = self._ctrl_eq * self._f_eq + self._ctrl_lin * self._f_lin + self._ctrl_robust * self._f_robust self.publish_control_wrench(self._tau) self._prev_t = t
def main(): rclpy.init() sim_time_param = is_sim_time() node = rclpy.create_node( 'set_timed_current_perturbation', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True, parameter_overrides=[sim_time_param]) # sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True) # node.set_parameters([sim_time]) node.get_logger().info('Starting current perturbation node') node.get_logger().info('Programming the generation of a current perturbation') starting_time = 0.0 if node.has_parameter('starting_time'): starting_time = node.get_parameter('starting_time').value if starting_time < 0.0: node.get_logger().warn('Negative starting time, setting it to 0.0') starting_time = 0.0 node.get_logger().info('Starting time=', starting_time) end_time = -1 if node.has_parameter('end_time'): end_time = node.get_parameter('end_time').value if end_time > 0 and end_time <= starting_time: raise RuntimeError('End time is smaller than the starting time') node.get_logger().info('End time=', (end_time if end_time > 0 else 'Inf.')) vel = 0.0 if node.has_parameter('current_velocity'): vel = node.get_parameter('current_velocity').value node.get_logger().info('Current velocity [m/s]=', vel) horz_angle = 0.0 if node.has_parameter('horizontal_angle'): horz_angle = node.get_parameter('horizontal_angle').value horz_angle *= pi / 180 node.get_logger().info('Current horizontal angle [deg]=', horz_angle * 180 / pi) vert_angle = 0.0 if node.has_parameter('vertical_angle'): vert_angle = node.get_parameter('vertical_angle').value vert_angle *= pi / 180 node.get_logger().info('Current vertical angle [deg]=', horz_angle * 180 / pi) #Create client service srv_name = '/hydrodynamics/set_current_velocity' set_current = node.create_client(SetCurrentVelocity, srv_name) if not set_current.wait_for_service(timeout_sec=20) node.get_logger().error('%s service not available! Closing node...' %(srv_name)) sys.exit(-1) # Wait to set the current model FREQ = 100 rate = node.create_rate(FREQ) thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) thread.start() while time_in_float_sec(node.get_clock().now()) < starting_time: if 1.0 / FREQ < starting_time: rate.sleep() # Wait to set the current model # rate = node.create_rate(100) # if rospy.get_time() < starting_time: # while rospy.get_time() < starting_time: # rate.sleep() node.get_logger().info('Applying current model...') req = SetCurrentVelocity.Request() req.vel = vel req.horz_angle = horz_angle req.vert_angle = vert_angle future = set_current.call_async(req) #NB : spining is done from another thread while not future.done(): try: response = future.result() except Exception as e: node.get_logger().error('Service call ' + srv_name + ' failed, error=' + str(e)): else: t = time_in_float_sec(node.get_clock().now().get_time()) node.get_logger().info('Current velocity changed successfully at %f s! vel= %f m/s' % (t, vel)) # if set_current(vel, horz_angle, vert_angle): # print('Current velocity changed successfully at %f s! vel= %f m/s' % (rospy.get_time(), vel)) # else: # print('Failed to change current velocity') if end_time > 0: while time_in_float_sec(node.get_clock().now()) < end_time: if 1.0 / FREQ < end_time: rate.sleep() # while time_in_float_sec(node.get_clock().now()) < end_time: # rate.sleep() req.vel = 0 req.horz_angle = horz_angle req.vert_angle = vert_angle node.get_logger().info('TIMEOUT, setting current velocity to zero...') future = set_current.call_async(req) #NB : spining is done from another thread while not future.done(): try: response = future.result() except Exception as e: node.get_logger().error('Service call ' + srv_name + ' failed, error=' + str(e)): #set_current.call(req) node.get_logger().info('Leaving node...') node.destroy_node() rclpy.shutdown() thread.join() #============================================================================== if __name__ == '__main__': try: main() except Exception as e: print('Something went wrong: ' + str(e)) finally: if rclpy.ok(): rclpy.shutdown()
def main(): rclpy.init() sim_time_param = is_sim_time() node = rclpy.create_node( 'send_waypoint_file', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True, parameter_overrides=[sim_time_param]) node.get_logger().info('Send a waypoint file, namespace=%s' % node.get_namespace()) if node.has_parameter('filename'): filename = node.get_parameter( 'filename').get_parameter_value().string_value else: raise RuntimeError('No filename found') # Important...ensure the clock has been updated when using sim time while node.get_clock().now() == rclpy.time.Time( clock_type=node.get_clock().clock_type): rclpy.spin_once(node) # If no start time is provided: start *now*. start_time = time_in_float_sec(node.get_clock().now()) start_now = True if node.has_parameter('start_time'): start_time = node.get_parameter('start_time').value if start_time < 0.0: node.get_logger().warn('Negative start time, setting it to 0.0') start_time = 0.0 start_now = True else: start_now = False node.get_logger().info('Start time=%.2f s' % start_time) interpolator = node.get_parameter_or( 'interpolator', 'lipb').get_parameter_value().string_value srv_name = 'init_waypoints_from_file' init_wp = node.create_client(InitWaypointsFromFile, srv_name) ready = init_wp.wait_for_service(timeout_sec=30) if not ready: raise RuntimeError('Service not available! Closing node...') (sec, nsec) = float_sec_to_int_sec_nano(start_time) req = InitWaypointsFromFile.Request() req.start_time = rclpy.time.Time(seconds=sec, nanoseconds=nsec).to_msg() req.start_now = start_now req.filename = String(data=filename) req.interpolator = String(data=interpolator) future = init_wp.call_async(req) rclpy.spin_until_future_complete(node, future) try: response = future.result() except Exception as e: node.get_logger().error('Service call ' + srv_name + ' failed, error=' + str(e)) else: node.get_logger().info( 'Waypoints file successfully received, ' 'filename=%s', filename) node.destroy_node()
def main(): rclpy.init() sim_time_param = is_sim_time() node = rclpy.create_node( 'set_link_wrench', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True, parameter_overrides=[sim_time_param]) node.get_logger().info('Apply programmed perturbation to vehicle ' + node.get_namespace()) starting_time = 0.0 if node.has_parameter('starting_time'): starting_time = float(node.get_parameter('starting_time').value) node.get_logger().info('Starting time in = {} s'.format(starting_time)) duration = 0.0 if node.has_parameter('duration'): duration = float(node.get_parameter('duration').value) # Compare to eps ? if duration == 0.0: node.get_logger().info('Duration not set, leaving node...') sys.exit(-1) node.get_logger().info('Duration [s]=' + ('Inf.' if duration < 0 else str(duration))) force = [0.0, 0.0, 0.0] if node.has_parameter('force'): force = node.get_parameter('force').value if len(force) != 3: raise RuntimeError('Invalid force vector') # Ensure type is float force = [float(elem) for elem in force] node.get_logger().info('Force [N]=' + str(force)) torque = [0.0, 0.0, 0.0] if node.has_parameter('torque'): torque = node.get_parameter('torque').value if len(torque) != 3: raise RuntimeError('Invalid torque vector') # Ensure type is float torque = [float(elem) for elem in torque] node.get_logger().info('Torque [N]=' + str(torque)) gazebo_ns = 'gazebo' if node.has_parameter('gazebo_ns'): gazebo_ns = node.get_parameter('gazebo_ns').value if not gazebo_ns.startswith('/'): gazebo_ns = '/' + gazebo_ns service_name = '{}/apply_link_wrench'.format(gazebo_ns) try: apply_wrench = node.create_client(ApplyLinkWrench, service_name) except Exception as e: node.get_logger().error('Creation of service ' + service_name + ' failed, error=' + str(e)) sys.exit(-1) try: ready = apply_wrench.wait_for_service(timeout_sec=10) if not ready: raise RuntimeError('') except: node.get_logger().error('Service ' + service_name + ' not available! Closing node...') sys.exit(-1) ns = node.get_namespace().replace('/', '') body_name = '%s/base_link' % ns FREQ = 100 rate = node.create_rate(FREQ) thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True) thread.start() while time_in_float_sec(node.get_clock().now()) < starting_time: # Just a guard for really short timeouts if 1.0 / FREQ < starting_time: rate.sleep() (d_secs, d_nsecs) = float_sec_to_int_sec_nano(duration) apw = ApplyLinkWrench.Request() apw.link_name = body_name apw.reference_frame = 'world' apw.reference_point = Point(x=0.0, y=0.0, z=0.0) apw.wrench = Wrench() apw.wrench.force = Vector3(x=force[0], y=force[1], z=force[2]) apw.wrench.torque = Vector3(x=torque[0], y=torque[1], z=torque[2]) apw.start_time = node.get_clock().now().to_msg() apw.duration = rclpy.time.Duration(seconds=d_secs, nanoseconds=d_nsecs).to_msg() future = apply_wrench.call_async(apw) # NB : spining is done from another thread while rclpy.ok(): if future.done(): try: response = future.result() except Exception as e: node.get_logger().error( 'Could not apply link wrench, exception thrown: %r' % (e, )) else: if response.success: message = '\nLink wrench perturbation applied!' message += '\n\tFrame: ' + body_name message += '\n\tDuration [s]: ' + str(duration) message += '\n\tForce [N]: ' + str(force) message += '\n\tTorque [Nm]: ' + str(torque) node.get_logger().info(message) else: message = 'Could not apply link wrench' message += '\n\tError is: %s' % (response.status_message) node.get_logger().error(message) finally: break node.destroy_node()