def get_config(self):
"""Get config data from ros param server."""
param_dict = {
'check_bumblebee': 'check_sensors/bumblebee',
'check_profiler': 'check_sensors/profiler',
'check_micron': 'check_sensors/micron'
}
cola2_ros_lib.getRosParams(self, param_dict, self.name)
def get_config(self):
param_dict = {
'sample': '/hierarchical/record/sample',
'landmark_id': '/hierarchical/record/landmark_id',
'interval_time': '/hierarchical/record/interval_time',
'base_pose': '/arm_controller/base_pose',
'base_ori': '/arm_controller/base_ori'
}
cola2_ros_lib.getRosParams(self, param_dict)
def get_config(self):
""" Read parameters from ROS Param Server."""
param_dict = {
'min_altitude': 'safe_depth_altitude/min_altitude',
'max_depth': 'safe_depth_altitude/max_depth',
'min_battery_charge': 'safety/min_battery_charge',
'min_battery_voltage': 'safety/min_battery_voltage',
'min_imu_update': 'safety/min_imu_update',
'min_depth_update': 'safety/min_depth_update',
'min_altitude_update': 'safety/min_altitude_update',
'min_gps_update': 'safety/min_gps_update',
'min_dvl_update': 'safety/min_dvl_update',
'min_nav_update': 'safety/min_nav_update',
'min_wifi_update': 'safety/min_wifi_update',
'working_area_north_origin': 'virtual_cage/north_origin',
'working_area_east_origin': 'virtual_cage/east_origin',
'working_area_north_length': 'virtual_cage/north_longitude',
'working_area_east_length': 'virtual_cage/east_longitude',
'timeout': 'safety/timeout',
'min_modem_update': 'safety/min_modem_update',
'min_dvl_good_data': 'safety/min_dvl_good_data',
'max_temperatures_ids': 'safety/max_temperatures_ids',
'max_temperatures_values': 'safety/max_temperatures_values'
}
cola2_ros_lib.getRosParams(self, param_dict, self.name)
# Define min altitude and max depth
try:
client2 = dynamic_reconfigure.client.Client("safe_depth_altitude",
timeout=10)
client2.update_configuration({
"min_altitude": self.min_altitude,
"max_depth": self.max_depth
})
except rospy.exceptions.ROSException:
rospy.logerr('%s, Error modifying safe_depth_altitude params.',
self.name)
# Define virtual cage limits
try:
client3 = dynamic_reconfigure.client.Client("virtual_cage",
timeout=10)
client3.update_configuration({
"north_origin": self.working_area_north_origin,
"east_origin": self.working_area_east_origin,
"north_longitude": self.working_area_north_length,
"east_longitude": self.working_area_east_length,
"enable": True
})
except rospy.exceptions.ROSException:
rospy.logerr('%s, Error modifying virtual_cage params.', self.name)
def get_config(self):
""" Read parameters from ROS Param Server."""
param_dict = {
'north_origin': 'virtual_cage/north_origin',
'east_origin': 'virtual_cage/east_origin',
'north_longitude': 'virtual_cage/north_longitude',
'east_longitude': 'virtual_cage/east_longitude',
'enabled': 'virtual_cage/enabled'
}
cola2_ros_lib.getRosParams(self, param_dict, self.name)
def get_config(self):
"""Get configuration parameters from ROS param server."""
params = {
'altitude_mode': 'target_reacquisition/altitude_mode',
'track_length': 'target_reacquisition/track_length',
'number_of_views': 'target_reacquisition/number_of_views',
'track_speed': 'target_reacquisition/track_speed'
}
cola2_ros_lib.getRosParams(self, params, self.name)
self.number_of_views = int(self.number_of_views)
def getConfig(self):
param_dict = {
'filename': 'learning/record/complete/filename',
'numberSample': 'learning/record/complete/number_sample',
'landmark_id': 'learning/record/complete/landmark_id',
'goal_valve': 'learning/record/complete/goal_valve',
'base_pose': '/arm_controller/base_pose',
'base_ori': '/arm_controller/base_ori'
}
cola2_ros_lib.getRosParams(self, param_dict)
self.file = open(self.filename + "_" + str(self.numberSample) + ".csv",
'w')
def get_config(self):
"""
Load the configuration from the yaml file using the library
of cola2_ros_lib
"""
param_dict = {
'samples': '/hierarchical/analyzer/samples',
'valves': '/hierarchical/analyzer/valves',
'prefix_files': '/hierarchical/analyzer/prexi_files',
'time_min_subtask': '/hierarchical/analyzer/time_min_subtask',
'time_min_breakpoints':
'/hierarchical/analyzer/time_min_breakpoints'
}
cola2_ros_lib.getRosParams(self, param_dict)
def getConfig(self):
param_dict = {
'limit_distance_goal': 'work_area/distance_goal',
'limit_gamma': 'work_area/gamma',
'limit_alpha': 'work_area/alpha',
'limit_beta': 'work_area/beta',
'landmark_id': 'work_area/landmark_id',
'period': 'work_area/period',
'goal_valve': 'work_area/goal_valve',
'base_pose': '/arm_controller/base_pose',
'base_ori': '/arm_controller/base_ori',
'limit_enter_zone': 'work_area/limit_enter_zone',
'limit_wait_outsite': 'work_area/limit_wait_outsite',
'limit_outsite': 'work_area/limit_outsite'
}
cola2_ros_lib.getRosParams(self, param_dict)
def getconfig(self):
"""
This method load the configuration and initialize the publishers,
subscribers and tf broadcaster and publishers
"""
param_dict = {'period': '/valve_tracker/period',
'num_valves': '/valve_tracker/number_of_valves',
'name_valve_pose_ee': '/valve_tracker/name_valve_pose_ee',
'name_valve_ori_ee': '/valve_tracker/name_valve_ori_ee',
'landmark_id': '/valve_tracker/landmark_id',
'valve_dist_centre': '/valve_tracker/valve_dist_centre',
'valve_dist_centre_ee': '/valve_tracker/valve_dist_centre_ee',
'valve_id': '/valve_tracker/valve_id',
'publisher_landmark': '/valve_tracker/publisher_landmark'
}
cola2_ros_lib.getRosParams(self, param_dict)
def get_config(self):
""" Get config from param server """
param_dict = {'frame_id': 'recovery_actions/frame_id',
'emergency_surface_setpoints': 'recovery_actions/emergency_surface_setpoints',
'controlled_surface_depth': 'recovery_actions/controlled_surface_depth'}
if not cola2_ros_lib.getRosParams(self, param_dict, self.name):
self.bad_config_timer = rospy.Timer(rospy.Duration(0.4), self.bad_config_message)
def get_config(self):
"""Get config from ROS param server."""
param_dict = {
'max_depth': 'safe_depth_altitude/max_depth',
'min_altitude': 'safe_depth_altitude/min_altitude'
}
if not cola2_ros_lib.getRosParams(self, param_dict, self.name):
self.bad_config_timer = rospy.Timer(rospy.Duration(0.4),
self.bad_config_message)
def get_config(self):
""" Reads configuration from ROSPARAM SERVER """
param_dict = {
'set_pose_depth': 'safety_set_pose/set_pose_depth',
'set_pose_axis': 'safety_set_pose/set_pose_axis',
'desired_pose': 'safety_set_pose/desired_pose'
}
if not cola2_ros_lib.getRosParams(self, param_dict, self.name):
self.bad_config_timer = rospy.Timer(rospy.Duration(0.4),
self.bad_config_message)
def getConfig(self):
param_dict = {'nbData': 'learning/dmp/nbData',
'nbDataRepro': 'learning/dmp/nbDataRepro',
'nbDataExport': 'learning/dmp/nbDataExport',
'nbVar': 'learning/dmp/nbVar',
'nbStates': 'learning/dmp/nbStates',
'nbDataExport': 'learning/dmp/nbDataExport',
'dt': 'learning/dmp/dt',
'kPmin': 'learning/dmp/kPmin',
'kPmax': 'learning/dmp/kPmax',
'Automaticks': 'learning/dmp/AutomaticKs',
'kP': 'learning/dmp/kP',
'kV': 'learning/dmp/kV',
'alpha': 'learning/dmp/alpha',
'demonstration_file': 'learning/dmp/demonstration_file',
'demonstrations': 'learning/dmp/demonstrations',
'export_filename': 'learning/dmp/export_filename',
}
cola2_ros_lib.getRosParams(self, param_dict)
self.kPmin_auv_x = np.asarray(self.kPmin)
self.kPmax_auv_x = np.asarray(self.kPmax)
def get_config(self):
""" Get config from param server """
param_dict = {'dvl_rotation': 'sim_dvl/dvl_rotation',
'dvl_type': 'sim_dvl/dvl_type',
'asymmetry': 'sim_dvl/asymmetry',
'mass': 'sim_dvl/mass',
'linear_damping': 'sim_dvl/linear_damping',
'quadratic_damping': 'sim_dvl/quadratic_damping',
'thruster_thrust': 'sim_dvl/thruster_thrust',
'bottom_depth': 'sim_dvl/bottom_depth'}
if not cola2_ros_lib.getRosParams(self, param_dict, self.name):
self.bad_config_timer = rospy.Timer(rospy.Duration(0.4), self.bad_config_message)
self.dvl_rotation = PyKDL.Rotation.RPY(self.dvl_rotation[0] * math.pi / 180.0, self.dvl_rotation[1] * math.pi / 180.0, self.dvl_rotation[2] * math.pi / 180.0)
def get_config(self):
'''
Load the file parameters.
'''
param_dict = {
'rate':
'current_estimation/rate',
'time_analize':
'current_estimation/time_analize',
'regular_force':
'current_estimation/regular_force',
'stare_landmark_enabled':
'current_estimation/stare_landmark_enabled',
'stare_landmark_id':
'current_estimation/stare_landmark_id',
'stare_landmark_offset':
'current_estimation/stare_landmark_offset',
'stare_landmark_tolerance':
'current_estimation/stare_landmark_tolerance',
'stare_landmark_keep_pose':
'current_estimation/stare_landmark_keep_pose'
}
cola2_ros_lib.getRosParams(self, param_dict)
rospy.loginfo('Time analize ' + str(self.time_analize))
def get_config(self):
""" Get config from param server """
param_dict = {
'max_pos': 'teleoperation/max_pos',
'min_pos': 'teleoperation/min_pos',
'max_vel': 'teleoperation/max_vel',
'min_vel': 'teleoperation/min_vel',
'pose_controlled_axis': 'teleoperation/pose_controlled_axis',
'base_pose': 'teleoperation/base_pose',
'actualize_base_pose': 'teleoperation/actualize_base_pose',
'robot_name': '/navigator/robot_frame_id'
}
if not cola2_ros_lib.getRosParams(self, param_dict, self.name):
rospy.logfatal("%s: shutdown due to invalid config parameters!",
self.name)
exit(0) # TODO: find a better way
def get_config(self):
""" Get config from param server """
if rospy.has_param(
"vehicle_name"): # This parameter is in dynamics yaml
self.vehicle_name = rospy.get_param('vehicle_name')
else:
rospy.logfatal("%s: vehicle_name parameter not found", self.name)
exit(0) # TODO: find a better way
param_dict = {
'latitude':
"dynamics/" + self.vehicle_name + "/ned_origin_latitude",
'longitude':
"dynamics/" + self.vehicle_name + "/ned_origin_longitude",
'odom_topic_name':
"dynamics/" + self.vehicle_name + "/odom_topic_name",
'altitude_range_topic_name':
"dynamics/" + self.vehicle_name + "/altitude_range_topic_name",
'world_frame_id': "dynamics/" + self.vehicle_name +
"/world_frame_id", # TODO: Shouldn't this be general for all nodes?
'dvl_type': "sim_nav_sensors/dvl_type",
'water_density': "sim_nav_sensors/water_density",
'imu_tf_array': "sim_nav_sensors/imu/tf",
'dvl_tf_array': "sim_nav_sensors/dvl/tf",
'gps_tf_array': "sim_nav_sensors/gps/tf",
'imu_period': "sim_nav_sensors/imu/period",
'dvl_period': "sim_nav_sensors/dvl/period",
'sea_bottom_depth': "sea_bottom_depth",
'gps_period': "sim_nav_sensors/gps/period",
'imu_orientation_covariance':
"sim_nav_sensors/imu/orientation_covariance",
'dvl_velocity_covariance':
"sim_nav_sensors/dvl/velocity_covariance",
'gps_position_covariance':
"sim_nav_sensors/gps/position_covariance"
}
if not cola2_ros_lib.getRosParams(self, param_dict, self.name):
rospy.logfatal("%s: shutdown due to invalid config parameters!",
self.name)
exit(0) # TODO: find a better way
self.imu_tf = __compute_tf__(np.array(self.imu_tf_array) * -1.0)
self.dvl_tf = __compute_tf__(np.array(self.dvl_tf_array) * -1.0)
'state_max': 'learning/auto/state_max',
'step_kp': 'learning/auto/step_kp',
'step_state': 'learning/auto/step_state'
'alpha': 'learning/auto/alpha',
'nbVar': 'learning/auto/nbVar',
'demonstration_file': 'learning/auto/demonstration_file',
'demonstrations': 'learning/auto/demonstrations',
'param_value': 'learning/auto/param_value',
'param_samples': 'learning/auto/param_samples',
'init_time': 'learning/auto/init_time',
'end_time': 'learning/auto/end_time',
'interval_time': 'learning/auto/interval_time',
'learning_export_file': 'learning/auto/learning_export_file',
'nbDataRepro': 'learning/reproductor/complete/nbDataRepro',
}
cola2_ros_lib.getRosParams(self, param_dict)
def reproduce_trajectory(self):
pass
def compare_trajectory(self):
pass
def learn_and_reproduce(self, kp_min, kp_max, states):
'''
The parameters are learned using the Dmp parameters and not exported
'''
dof_list = [1,0,0,1,0,0,0,0,0,0]
dmp_auv_x_yaw = LearningDmpGeneric(-99.0, -99.0, kp_min, kp_max,
self.alpha, states, dof_list,
nbData, self.demonstration_file,
def get_config(self):
""" Get config from config file """
if rospy.has_param("vehicle_name"):
self.vehicle_name = rospy.get_param('vehicle_name')
else:
rospy.logfatal("%s: vehicle_name parameter not found", self.name)
exit(0) # TODO: find a better way
param_dict = {
'thrusters':
"dynamics/" + self.vehicle_name + "/number_of_thrusters",
'force_topic': "dynamics/" + self.vehicle_name + "/force_topic",
'use_force_topic':
"dynamics/" + self.vehicle_name + "/use_force_topic",
'thrusters_topic':
"dynamics/" + self.vehicle_name + "/thrusters_topic",
'thrusters_matrix':
"dynamics/" + self.vehicle_name + "/thrusters_matrix",
'fins': "dynamics/" + self.vehicle_name + "/number_of_fins",
'fins_topic': "dynamics/" + self.vehicle_name + "/fins_topic",
'a_fins': "dynamics/" + self.vehicle_name + "/a_fins",
'k_cd_fins': "dynamics/" + self.vehicle_name + "/k_cd_fins",
'k_cl_fins': "dynamics/" + self.vehicle_name + "/k_cl_fins",
'period': "dynamics/" + self.vehicle_name + "/period",
'mass': "dynamics/" + self.vehicle_name + "/mass",
'buoyancy': "dynamics/" + self.vehicle_name + "/buoyancy",
'gravity_center':
"dynamics/" + self.vehicle_name + "/gravity_center",
'g': "dynamics/" + self.vehicle_name + "/g",
'radius': "dynamics/" + self.vehicle_name + "/radius",
'max_thrusters_rpm':
"dynamics/" + self.vehicle_name + "/max_thrusters_rpm",
'max_fins_angle':
"dynamics/" + self.vehicle_name + "/max_fins_angle",
'ctf': "dynamics/" + self.vehicle_name + "/ctf",
'ctb': "dynamics/" + self.vehicle_name + "/ctb",
'dzv': "dynamics/" + self.vehicle_name + "/dzv",
'dv': "dynamics/" + self.vehicle_name + "/dv",
'dh': "dynamics/" + self.vehicle_name + "/dh",
'density': "dynamics/" + self.vehicle_name + "/density",
'tensor': "dynamics/" + self.vehicle_name + "/tensor",
'damping': "dynamics/" + self.vehicle_name + "/damping",
'quadratic_damping':
"dynamics/" + self.vehicle_name + "/quadratic_damping",
'p_0': "dynamics/" + self.vehicle_name + "/initial_pose",
'v_0': "dynamics/" + self.vehicle_name + "/initial_velocity",
'odom_topic_name':
"dynamics/" + self.vehicle_name + "/odom_topic_name",
'frame_id': "dynamics/" + self.vehicle_name + "/frame_id",
'world_frame_id':
"dynamics/" + self.vehicle_name + "/world_frame_id",
'current_mean': "dynamics/current_mean",
'current_sigma': "dynamics/current_sigma",
'current_min': "dynamics/current_min",
'current_max': "dynamics/current_max",
'current_enabled': "dynamics/current_enabled",
'collisions_topic':
"dynamics/" + self.vehicle_name + "/uwsim_contact_sensor",
'sea_bottom_depth': "sea_bottom_depth"
}
if not cola2_ros_lib.getRosParams(self, param_dict, self.name):
rospy.logfatal("%s: shutdown due to invalid config parameters!",
self.name)
exit(0) # TODO: find a better way
if len(self.collisions_topic) > 0:
self.contact_sensor_available = True
self.thrusters_matrix = np.array(self.thrusters_matrix).reshape(
6, self.thrusters)
