def __init__(self):
self.node_name = 'multi_target_tracking_node'
rospy.init_node(self.node_name, argv=sys.argv)
# Try connecting to global dynamic reconfigure server, otherwise create local one
self.config = {}
try:
self.client = Client('/gcs/multi_target_tracking_config_server',
config_callback=self.config_callback,
timeout=1)
rospy.loginfo('Connected to remote dynamic reconfigure server.')
except rospy.ROSException:
rospy.logwarn('Failed to connect to dynamic reconfigure server.')
rospy.loginfo('Connected to local dynamic reconfigure server.')
self.server = Server(MultiTargetTrackingNodeConfig,
callback=self.config_callback)
self.client = Client(self.node_name)
self.config = self.client.get_configuration()
detections_pub_topic = self.node_name + '/detections'
self.detections_pub = rospy.Publisher(detections_pub_topic,
Detection3DArray,
queue_size=1)
pose_cov_topic = self.node_name + '/pose_cov'
self.pose_cov_pub = rospy.Publisher(pose_cov_topic,
PoseWithCovarianceStamped,
queue_size=1)
num_target_topic = self.node_name + '/num_targets'
self.num_target_pub = rospy.Publisher(num_target_topic,
Int32,
queue_size=1)
self.dbscan = DBSCAN(eps=1.0, min_samples=1)
self.dimensions = 2
from vswarm.object_tracking.filters import \
extended_gmphd_filter_2d_polar as gmphd_filter
self.gmphd = gmphd_filter.get_filter()
self.velocity = None
velocity_topic = 'mavros/local_position/velocity_body'
self.velocity_sub = rospy.Subscriber(velocity_topic,
TwistStamped,
callback=self.velocity_callback,
queue_size=1)
self.last_detections_msg = None
detection_sub_topic = 'detections'
self.detections_sub = rospy.Subscriber(
detection_sub_topic,
Detection3DArray,
callback=self.detections_callback,
queue_size=1)
def __init__(self):
self.node_name = 'relative_localization_node'
rospy.init_node(self.node_name, argv=sys.argv)
# Try connecting to global dynamic reconfigure server, otherwise create local one
self.config = {}
try:
self.client = Client('/gcs/relative_localization_config_server',
config_callback=self.config_callback,
timeout=1)
rospy.loginfo('Connected to remote dynamic reconfigure server.')
except rospy.ROSException:
rospy.logwarn('Failed to connect to dynamic reconfigure server.')
rospy.loginfo('Connected to local dynamic reconfigure server.')
self.server = Server(RelativeLocalizationNodeConfig,
callback=self.config_callback)
self.client = Client(self.node_name)
self.config = self.client.get_configuration()
# Calibration paramters (one for each camera namespace)
calibrations_dict = rospy.get_param('~calibration')
self.localizers = {}
for camera_ns, calibration_dict in calibrations_dict.items():
intrinsics = calibration_dict['intrinsics']
D = calibration_dict[
'distortion_coeffs'] # Assume equidistant/fisheye
# Camera matrix from intrinsic parameters
fx, fy, cx, cy = intrinsics
K = np.array([[fx, 0., cx], [0., fy, cy], [0., 0., 1.]])
# Distortion coefficients
D = np.array(D)
self.localizers[camera_ns] = RelativeLocalizer(K, D)
# Transform
self.buffer = tf2_ros.Buffer(rospy.Duration(1.0))
self.listener = tf2_ros.TransformListener(self.buffer)
# Publisher
detections_pub_topic = self.node_name + '/detections'
self.detections_pub = rospy.Publisher(detections_pub_topic,
Detection3DArray,
queue_size=1)
# Subscriber
self.detections_sub = rospy.Subscriber(
'detections', Detection2DArray, callback=self.detections_callback)
def _update_move_base_params(self, config):
"""
If parameter updates have not been called in the last 5 seconds allow the
subscriber callback to set them
"""
if ((rospy.Time.now().to_sec() - self.last_move_base_update) > 5.0):
self.update_base_local_planner = True
if self.update_base_local_planner:
self.update_base_local_planner = False
self.last_move_base_update = rospy.Time.now().to_sec()
try:
dyn_reconfigure_client = Client(
"/move_base/TrajectoryPlannerROS", timeout=1.0)
changes = dict()
changes['acc_lim_x'] = minimum_f(
self.valid_config.accel_limit_mps2,
self.valid_config.decel_limit_mps2)
changes[
'acc_lim_theta'] = self.valid_config.yaw_accel_limit_rps2
changes['max_vel_x'] = self.valid_config.vel_limit_mps
changes['max_vel_theta'] = self.valid_config.yaw_rate_limit_rps
changes[
'min_vel_theta'] = -self.valid_config.yaw_rate_limit_rps
dyn_reconfigure_client.update_configuration(changes)
dyn_reconfigure_client.close()
except:
pass
try:
dyn_reconfigure_client = Client("/move_base/DWAPlannerROS",
timeout=1.0)
changes = dict()
changes['acc_lim_x'] = minimum_f(
self.valid_config.accel_limit_mps2,
self.valid_config.decel_limit_mps2)
changes['acc_lim_th'] = self.valid_config.yaw_accel_limit_rps2
changes['max_vel_x'] = self.valid_config.vel_limit_mps
changes['max_rot_vel'] = self.valid_config.yaw_rate_limit_rps
dyn_reconfigure_client.update_configuration(changes)
dyn_reconfigure_client.close()
except:
pass
rospy.loginfo(
"Segway Driver updated move_base parameters to match machine parameters"
)
def __init__(self):
rospy.init_node('rtcrobot') #, anonymous=True
self.uuidnavigation = roslaunch.rlutil.get_or_generate_uuid(
None, False)
#roslaunch.configure_logging(self.uuid)
self.launchnavigation = roslaunch.parent.ROSLaunchParent(
self.uuidnavigation,
["/home/uzi/catkin_ws/src/rtcrobot/launch/navigation.launch"])
self.isnavigation = False
self.isActive = False
self.rate = 50
#self._cfg = RTCRobotConfig()
s = rospy.Service('rtcrobot_start', std_srvs.srv.Empty,
self.rtcrobot_start)
s = rospy.Service('rtcrobot_stop', std_srvs.srv.Empty,
self.rtcrobot_stop)
#* Dynamic parameter
dynamic_configure_server = Server(RTCRobotConfig,
self.dynamic_callback)
dynamic_configure_client = Client('rtcrobot')
# params = { 'int_param' : '60', 'double_param' : .6 }
# config = dynamic_configure_client.update_configuration(params)
#*Run Webserver
uuidwebserver = roslaunch.rlutil.get_or_generate_uuid(None, False)
uuidwebserver = roslaunch.parent.ROSLaunchParent(
uuidwebserver,
["/home/uzi/catkin_ws/src/rtcrobot/launch/webserver.launch"])
uuidwebserver.start()
rospy.loginfo("OK")
def __init__(self,
srv_name="/speech_recognition",
node_name="/speech_recognition",
timeout=10):
self._sr_srv = rospy.ServiceProxy(srv_name, SpeechRecognition)
self._sr_srv.wait_for_service(timeout=timeout)
self._cfg = Client(node_name, timeout=timeout)
def __init__(self, max_window_size = 75,step=5):
smach.State.__init__(self,
outcomes=['SETUP_DONE', 'FINISH'],
input_keys=['counter_in','acc_results', 'cam_results', 'odom_results', 'lidar_results', 'mic_results','imu_results', 'result_cum', 'results_', 'x_array'],
output_keys=['counter_out','acc_results', 'cam_results', 'odom_results', 'lidar_results', 'mic_results','imu_results','result_cum', 'results_', 'x_array'])
#rospy.spin()
self.step = step
self.max_window_size = max_window_size
#self.acc_client = Client("accelerometer_process", timeout=3, config_callback=self.callback)
self.cam_client = Client("vision_utils_ros_android", timeout=3, config_callback=self.callback)
#self.odom_client = Client("odom_collisions", timeout=3, config_callback=self.callback)
self.lidar_client = Client("laser_collisions", timeout=3, config_callback=self.callback)
self.mic_client = Client("mic_collisions", timeout=3, config_callback=self.callback)
self.imu_client = Client("imu_collision_detection", timeout=3, config_callback=self.callback)
rospy.sleep(0.2)
def __init__(self, frame_id='enu', map_size=500, resolution=0.3, rate=1):
self.frame_id = frame_id
self.ogrids = {}
self.odom = None
# Some default values
self.plow = True
self.plow_factor = 0
self.ogrid_min_value = -1
self.draw_bounds = False
self.resolution = resolution
self.enforce_bounds = False
self.enu_bounds = [[0, 0, 1], [0, 0, 1], [0, 0, 1], [0, 0, 1]]
# Default to centering the ogrid
position = np.array(
[-(map_size * resolution) / 2, -(map_size * resolution) / 2, 0])
quaternion = np.array([0, 0, 0, 1])
self.origin = navigator_tools.numpy_quat_pair_to_pose(
position, quaternion)
self.global_ogrid = self.create_grid((map_size, map_size))
set_odom = lambda msg: setattr(
self, 'odom', navigator_tools.pose_to_numpy(msg.pose.pose))
rospy.Subscriber('/odom', Odometry, set_odom)
self.publisher = rospy.Publisher('/ogrid_master',
OccupancyGrid,
queue_size=1)
Server(OgridConfig, self.dynamic_cb)
dynam_client = Client("bounds_server", config_callback=self.bounds_cb)
rospy.Timer(rospy.Duration(1.0 / rate), self.publish)
def __init__(self, namespace, debug=False):
print(namespace)
image_topic = rospy.get_param(namespace + "/topic")
self.client_name = rospy.get_param(namespace + "/client")
self.p_lower_nth_default = rospy.get_param(namespace +
'/percentile_nth_min')
self.p_upper_nth_default = rospy.get_param(namespace +
'/percentile_nth_max')
self.p_lower_default = rospy.get_param(namespace + '/percentile_lower')
self.p_upper_default = rospy.get_param(namespace + '/percentile_upper')
self.exposure_default = rospy.get_param(namespace +
'/exposure_default')
self.p_lower_nth_default = int(self.p_lower_nth_default)
self.p_upper_nth_default = int(self.p_upper_nth_default)
self.p_upper_default = int(self.p_upper_default)
self.p_lower_default = int(self.p_lower_default)
self.debug = debug
print("Topic: " + str(image_topic))
print("Client: " + str(self.client_name))
print("Percentile nth min: " + str(self.p_lower_nth_default))
print("Percentile nth max: " + str(self.p_upper_nth_default))
print("Percentile lower: " + str(self.p_lower_default))
print("Percentile upper: " + str(self.p_upper_default))
print("Exposure_default: " + str(self.exposure_default))
rospy.Subscriber(image_topic, CompressedImage, self.image_callback)
self.PATH_PKG = os.path.dirname(os.path.abspath(__file__))
self.image = None
self.client = Client(self.client_name)
self.sub_sampling = 0.25
self.stat = Statistics()
def main():
# server_name = rospy.resolve_name('~data_collection_server')
# root_dir = osp.expanduser(rospy.get_param('~root_dir'))
server_name = '/data_collection_server'
root_dir = osp.expanduser('~/.ros/instance_occlsegm')
caller = rospy.ServiceProxy('%s/save_request' % server_name, Trigger)
client = Client(server_name)
action = 'n' # to initialize
while not rospy.is_shutdown():
while action not in ['n', 's', 'q']:
action = six.input(
'Please select actions: [n] New save_dir, [s] Save, [q] Quit: '
)
action = action.strip().lower()
if action == 'n':
now = rospy.Time.now()
save_dir = osp.join(root_dir, str(now.to_nsec()))
client.update_configuration({'save_dir': save_dir})
print('-' * 79)
print('Updated save_dir to: {}'.format(save_dir))
elif action == 's':
res = caller.call()
print('Sent request to data_collection_server')
print(res)
else:
assert action == 'q'
return
action = None
def __init__(self):
# Algorithms
self.ros = ROS(ros_callback=self.ros_callback,
ui_callback=self.ui_callback,
mode="Pipeline")
self.cvAverage = CvAverage(width=394, height=480, maxSize=2)
self.lkSonar = LK(sonar=True, points_wanted=5)
self.lkCamera = LK(sonar=False, points_wanted=2)
self.featureExtractor = FeatureExtractor()
# Other variables
self.currTime = 0
self.sonarImageMouse = (0, 0)
self.cameraImageMouse = (0, 0)
self.mode = 2
# Data capture
self.data_list = []
self.currDirectory = os.getcwd()
self.writeDirectory = "/"
# FYP Algorithms
self.mapper = Mapper()
self.pf = ParticleFilter(Sonar_resolution=(394, 480),
Camera_resolution=(780, 580),
Npop_particles=2000)
srv = Server(PipelineConfig, self.dynamic_callback)
self.client = Client("Pipeline", timeout=30, config_callback=None)
def on_enter(self, userdata):
self._clients = {}
try:
for server in self._param.keys():
self._clients[server] = Client("/trajectory_controllers/" + userdata.traj_controller[0] + "/" + server + "/" + userdata.traj_controller[1])
except Exception as e:
Logger.logwarn('Was unable to reach parameter server:\n%s' % str(e))
self._failed = True
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.last_pose = None
self.ndt_pose_sub = rospy.Subscriber('/ndt_pose',
PoseStamped,
self.ndt_pose_cb,
queue_size=1)
self.twist_gain_dyn_client = Client('/twist_gains/twist_gains')
def __init__(self):
self.clt = Client("drive_param_server",
timeout=30,
config_callback=self.param_server_callback)
self.drive_state = 0
self.speed_setting = 2 # default to medium speed
self.cmd_vel_pub = rospy.Publisher("teleop/cmd_vel",
Twist,
queue_size=10)
self.joy_sub = rospy.Subscriber("joy", Joy, self.on_joy)
def __init__(self):
self.classifier_path = rospy.get_param('~classifier_path')
self.feature_path = rospy.get_param('~feature_path')
self.labels_path = rospy.get_param('~labels_path')
self.action_server = actionlib.SimpleActionServer(
'labels2Features', labels2FeaturesAction,
self.labels2Features_actionlib_callback, False)
self.action_server.start()
print 'ActionLib server started'
self.client = Client('classifier',
timeout=30) #dynamic_reconfig for client
def __init__(self):
#self.launch = roslaunch.scriptapi.ROSLaunch()
#self.launch.start()
managed_launches = map(ManagedLaunch.factory, launches)
self.managed_launches = dict([(ml.name, ml)
for ml in managed_launches])
self.config = None
self.srv = Server(GettingStartedConfig, self.dynparam_cb)
self.client = Client("getting_started_node")
def __init__(self, gait_type='walk_like', joint_list=None, max_time_step=0.1):
self._gait_type = gait_type
self._joint_list = joint_list
self._max_time_step = max_time_step
self.current_gains = [self.look_up_table(i) for i in range(len(self._joint_list))]
self.interpolation_done = True
self.last_update_time = 0
self._clients = []
for i in range(len(self._joint_list)):
self._clients.append(Client('/march/controller/trajectory/gains/' + self._joint_list[i], timeout=30))
rospy.Subscriber('/march/gait/schedule/goal', GaitActionGoal, callback=self.gait_selection_callback)
self._linearize = rospy.get_param('~linearize_gain_scheduling')
def init_dyn_recfg():
cli = tcc.TwistControllerReconfigureClient()
cli.init()
cli.set_config_param(tcc.CTRL_IF, tcc.TwistController_VELOCITY_INTERFACE)
cli.set_config_param(tcc.NUM_FILT, False)
cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY)
cli.set_config_param(tcc.LAMBDA_MAX, 0.1)
cli.set_config_param(tcc.W_THRESH, 0.05)
# cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_LEAST_SINGULAR_VALUE)
# cli.set_config_param(tcc.LAMBDA_MAX, 0.2)
# cli.set_config_param(tcc.EPS_DAMP, 0.2)
cli.set_config_param(tcc.EPS_TRUNC, 0.001)
cli.set_config_param(tcc.PRIO_CA, 100)
cli.set_config_param(tcc.PRIO_JLA, 50)
cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS)
cli.set_config_param(tcc.K_H, 1.0)
cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
cli.set_config_param(tcc.K_H_CA, -2.0)
cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0)
cli.set_config_param(tcc.CRIT_THRESH_CA, 0.025)
cli.set_config_param(tcc.DAMP_CA, 0.000001)
cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
cli.set_config_param(tcc.K_H_JLA, -1.0)
cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0)
cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0)
cli.set_config_param(tcc.DAMP_JLA, 0.00001)
cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
cli.set_config_param(tcc.KEEP_DIR, True)
cli.set_config_param(tcc.ENF_VEL_LIM, True)
cli.set_config_param(tcc.ENF_POS_LIM, True)
cli.update()
cli.close()
cli = Client('frame_tracker')
ft_param = {
'cart_min_dist_threshold_lin': 0.2,
'cart_min_dist_threshold_rot': 0.2
}
cli.update_configuration(ft_param)
cli.close()
def do_initial_config(self):
# Do initial configuration.
params = {
"DRIVE_SPEED": State.DRIVE_SPEED,
"REVERSE_SPEED": State.REVERSE_SPEED,
"TURN_SPEED": State.TURN_SPEED,
"HEADING_RESTORE_FACTOR": State.HEADING_RESTORE_FACTOR,
"GOAL_DISTANCE_OK": State.GOAL_DISTANCE_OK,
"ROTATE_THRESHOLD": State.ROTATE_THRESHOLD,
"DRIVE_ANGLE_ABORT": State.DRIVE_ANGLE_ABORT,
}
dyn_client = Client(self.rover + '_MOBILITY')
dyn_client.update_configuration(params)
print('Initial configuration sent.')
def __init__(self, joint_list):
self._gait_type = None
self._joint_list = joint_list
self.interpolation_done = True
self._last_update_times = []
self._clients = [
Client('/march/controller/trajectory/gains/' + joint, timeout=90)
for joint in self._joint_list
]
self.current_gains = []
rospy.Subscriber('/march/gait/schedule/goal',
GaitActionGoal,
callback=self.gait_selection_callback)
self._linearize = rospy.get_param('~linearize_gain_scheduling')
self._gradient = rospy.get_param('~linear_slope')
def init_dyn_recfg():
cli = tcc.TwistControllerReconfigureClient()
cli.init()
cli.set_config_param(tcc.CTRL_IF, tcc.TwistController_VELOCITY_INTERFACE)
cli.set_config_param(tcc.NUM_FILT, False)
cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_CONSTANT)
cli.set_config_param(tcc.DAMP_FACT, 0.2)
cli.set_config_param(tcc.EPS_TRUNC, 0.001)
cli.set_config_param(tcc.PRIO_CA, 100)
cli.set_config_param(tcc.PRIO_JLA, 50)
cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS)
cli.set_config_param(tcc.K_H, 1.0)
cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
cli.set_config_param(tcc.K_H_CA, -1.0)
cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
cli.set_config_param(tcc.ACTIV_BUF_CA, 25.0)
cli.set_config_param(tcc.CRIT_THRESH_CA, 0.025)
cli.set_config_param(tcc.DAMP_CA, 0.000001)
cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
cli.set_config_param(tcc.K_H_JLA, -1.0)
cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0)
cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0)
cli.set_config_param(tcc.DAMP_JLA, 0.00001)
cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_BASE_ACTIVE)
cli.set_config_param(tcc.EXT_RATIO, 0.05)
cli.set_config_param(tcc.KEEP_DIR, True)
cli.set_config_param(tcc.ENF_VEL_LIM, True)
cli.set_config_param(
tcc.ENF_POS_LIM,
False) # To show that joint pos limits are violated if possible.
cli.update()
cli.close()
cli = Client('frame_tracker')
ft_param = {
'cart_min_dist_threshold_lin': 5.0,
'cart_min_dist_threshold_rot': 5.0
}
cli.update_configuration(ft_param)
cli.close()
def __init__(self, joint_names):
"""Constructor"""
super(UpdateJointCalibrationState,
self).__init__(outcomes=['updated', 'failed'],
input_keys=['joint_offsets'])
self._joint_names = joint_names
self._failed = False
self._clients = list()
for i in range(len(self._joint_names)):
self._clients.append(
Client("/atlas_controller/calibration/" +
self._joint_names[i]))
self._pending_clients = list()
def on_enter(self, userdata):
self._clients = {}
self._waiting_for_response = [True] * len(self._param.keys())
self._parameter_value_list = [None] * sum(
map(len, self._param.values()))
try:
for server in self._param.keys():
self._clients[server] = Client("/trajectory_controllers/" +
userdata.traj_controller[0] +
"/" + server + "/" +
userdata.traj_controller[1])
except Exception as e:
Logger.logwarn('Was unable to reach parameter server:\n%s' %
str(e))
self._failed = True
def init_dyn_recfg():
cli = tcc.TwistControllerReconfigureClient()
cli.init()
cli.set_config_param(tcc.NUM_FILT, False)
cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY)
cli.set_config_param(tcc.LAMBDA_MAX, 0.1)
cli.set_config_param(tcc.W_THRESH, 0.05)
cli.set_config_param(tcc.EPS_TRUNC, 0.001)
cli.set_config_param(tcc.PRIO_CA, 50)
cli.set_config_param(
tcc.PRIO_JLA,
100) # change priorities now CA constraint has highest prio
cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS)
cli.set_config_param(tcc.K_H, 1.0)
cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
cli.set_config_param(tcc.K_H_CA, -1.0)
cli.set_config_param(tcc.DAMP_CA, 0.000001)
cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0)
cli.set_config_param(tcc.CRIT_THRESH_CA, 0.03)
cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
cli.set_config_param(tcc.K_H_JLA, -1.0)
cli.set_config_param(tcc.DAMP_JLA, 0.000001)
cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0)
cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0)
cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
cli.set_config_param(tcc.KEEP_DIR, True)
cli.set_config_param(tcc.ENF_VEL_LIM, True)
cli.set_config_param(tcc.ENF_POS_LIM, True)
cli.update()
cli.close()
cli = Client('frame_tracker')
ft_param = {
'cart_min_dist_threshold_lin': 0.2,
'cart_min_dist_threshold_rot': 0.2
}
cli.update_configuration(ft_param)
cli.close()
def connect_server(self):
if self.config.server == '':
self.dr_client = None
self.new_server = False
return True
try:
self.dr_client = Client(self.config.server,
timeout=0.05,
config_callback=self.server_dr_callback)
self.new_server = False
rospy.loginfo("connected to new server '" + self.config.server +
"'")
except Exception as ex:
rospy.logerr_throttle(
5.0, "no server available '" + self.config.server + "'")
return False
return True
def read_exposure():
time_avg = []
while not rospy.is_shutdown():
start = time.time()
client_name = "ueye_cam_nodelet_front"
client = Client(client_name)
params = {"exposure": 33.0}
client.update_configuration(params)
rospy.get_param("/" + str(client_name) + "exposure", None)
stop = time.time()
duration = stop - start
print(duration)
time_avg.append(duration)
print("Duration between call exposure API: ")
time_avg = np.array(time_avg)
print('max:', time_avg.max())
print('min:', time_avg.min())
print('mean:', time_avg.mean())
def __init__(self):
self.node_name = 'migration_node'
rospy.init_node(self.node_name, argv=sys.argv)
# Dynamic reconfigure
self.config = argparse.Namespace()
self.server = Server(MigrationNodeConfig,
callback=self.config_callback)
self.client = Client(self.node_name)
self.config = argparse.Namespace(**self.client.get_configuration())
self.n = rospy.get_param('/gcs/n')
rospy.loginfo('Got {} agents.'.format(self.n))
self.poses = rospy.get_param('~poses',
default='mavros/vision_pose/pose')
waypoints_list = rospy.get_param('~waypoints')
self.waypoints = [
np.array([w['x'], w['y'], w['z']]) for w in waypoints_list
]
self.current_waypoint = 0
migration_topic = 'migration_node/setpoint'
self.setpoint_pub = rospy.Publisher(migration_topic,
PoseStamped,
queue_size=1)
marker_topic = 'migration_node/migration_markers'
self.waypoint_pub = rospy.Publisher(marker_topic,
MarkerArray,
queue_size=1)
self.positions = [None] * self.n
self.pose_subscribers = []
for i in range(self.n):
topic = '/drone_{}/{}'.format(i + 1, self.poses)
pose_sub = rospy.Subscriber(topic,
PoseStamped,
callback=self.pose_callback,
callback_args=i)
self.pose_subscribers.append(pose_sub)
def init_dyn_recfg():
cli = tcc.TwistControllerReconfigureClient()
cli.init()
cli.set_config_param(tcc.CTRL_IF, tcc.TwistController_VELOCITY_INTERFACE)
cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY)
cli.set_config_param(tcc.LAMBDA_MAX, 0.1)
cli.set_config_param(tcc.W_THRESH, 0.05)
cli.set_config_param(tcc.PRIO_CA, 100)
cli.set_config_param(tcc.PRIO_JLA, 50)
cli.set_config_param(tcc.SOLVER, tcc.TwistController_GPM)
cli.set_config_param(tcc.K_H, 1.0)
cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
cli.set_config_param(tcc.K_H_CA, -2.0)
cli.set_config_param(tcc.DAMP_CA, 0.000001)
cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
cli.set_config_param(tcc.K_H_JLA, -1.0)
cli.set_config_param(tcc.DAMP_JLA, 0.00001)
cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
cli.set_config_param(tcc.KEEP_DIR, True)
cli.set_config_param(tcc.ENF_VEL_LIM, True)
cli.set_config_param(
tcc.ENF_POS_LIM,
False) # To show that joint pos limits are violated if possible.
cli.update()
cli.close()
cli = Client('frame_tracker')
ft_param = {
'cart_min_dist_threshold_lin': 0.2,
'cart_min_dist_threshold_rot': 0.2
}
cli.update_configuration(ft_param)
cli.close()
def __init__(self):
self.__pub_rate = rospy.get_param('~rate', 10) #Hz
self.__tf_broadcaster = tf2_ros.StaticTransformBroadcaster()
self.__static_transformStamped = geometry_msgs.msg.TransformStamped()
self.__srv = Server(TfConfig, self.__reconfigure_callback)
self.__static_transformStamped.header.frame_id = rospy.get_param('~header_frame', "world")
self.__static_transformStamped.child_frame_id = rospy.get_param('~child_frame', "base_link")
self.__yaml_path = rospy.get_param('~yaml', "")
if self.__yaml_path != "":
try:
yaml_file = open(self.__yaml_path, "r+")
yaml_data = yaml.load(yaml_file)
client = Client(rospy.get_name())
client.update_configuration(yaml_data)
self.__set_tf(yaml_data)
except IOError:
rospy.logwarn('file not found in' + self.__yaml_path)
rospy.logwarn("tf parameters are set 0.0")
self.__set_zero()
else:
self.__set_zero()
def main():
rospy.init_node("follow_joint_trajectory_action_server")
rospy.loginfo("%s started. Ctrl-c to stop." % rospy.get_name())
# Get ROS parameters
joint_names = rospy.get_param('~joint_names', ['joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5'])
ns = rospy.get_param('~namespace_of_topics', 'robot')
dyn_cfg_srv = Server(PositionJointTrajectoryActionServerConfig,
lambda config, level: config)
# Reflect ROS parameters to dynamic reconfigure server
client = Client(rospy.get_name())
client.update_configuration({('name_of_joint' + str(idx)):joint_name
for idx, joint_name in enumerate(joint_names)})
fjtas = FollowJointTrajectoryActionServer(joint_names, ns, dyn_cfg_srv)
def cleanup():
fjtas.clean_shutdown()
rospy.on_shutdown(cleanup)
rospy.spin()
def __init__(self, joint_list):
self._gait_type = None
self._joint_list = joint_list
self._last_update_times = []
self._clients = [
Client("/march/controller/trajectory/gains/" + joint, timeout=90)
for joint in self._joint_list
]
self.current_gains = []
rospy.Subscriber(
"/march/gait_selection/current_gait",
CurrentGait,
callback=self.gait_selection_callback,
)
rospy.Service(
"/march/gain_scheduling/get_configuration",
Trigger,
handler=self.configuration_cb,
)
self._linearize = rospy.get_param("~linearize_gain_scheduling")
self._gradient = rospy.get_param("~linear_slope")
self._configuration = rospy.get_param("~configuration")
rospy.loginfo(f"Exoskeleton was started with gain tuning for "
f"{self._configuration}")
