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 _update_move_base_params(self):
"""
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/DWAPlannerROS",timeout=1.0)
changes = dict()
changes['acc_lim_x'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2)
changes['acc_lim_y'] = maximum_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.x_vel_limit_mps
changes['max_vel_y'] = self.valid_config.y_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("Vector Driver updated move_base parameters to match machine parameters")
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):
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.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.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):
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 configure_prosilica():
print "Configuring prosilica"
client = ReconfigureClient("prosilica_driver")
client.update_configuration({
'auto_exposure': False,
'exposure': 0.4,
'auto_gain': False,
'gain': 0.0,
'auto_whitebalance': False,
'whitebalance_red': 114,
'whitebalance_blue': 300
})
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 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.')
class LaunchManager(object):
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 dynparam_cb(self, config, level):
rospy.logdebug("Reconfigure Request: (%i) %s" % (level, (str(config))))
rospy.logdebug("keys: %s" % str(config.keys()))
for name, ml in self.managed_launches.iteritems():
rospy.logdebug("cb: %s (%s)" % (name, str(ml)))
if config[name]:
args = []
if name.endswith("_map"):
args = ["map_filename:=%s" % config.map_filename]
rospy.loginfo("Map args: %s %s" % (name, args))
ml.start(args)
else:
ml.stop()
self.config = config
return config
def spin_once(self):
config_updates = {}
for name, ml in self.managed_launches.iteritems():
rospy.logdebug("Spin: check if %s has died..." % name)
if ml.died():
rospy.logwarn("Launch %s (%i) has died..." %
(name, ml.process.pid))
ml.process = None
config_updates[name] = False
if config_updates:
rospy.loginfo('updating config: %s' % str(config_updates))
#self.srv.update_configuration(config_updates)
self.client.update_configuration(config_updates)
def spin(self, rate=5.0):
r = rospy.Rate(rate)
while not rospy.is_shutdown():
self.spin_once()
r.sleep()
class Setup(smach.State):
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', 'imu_results', 'lidar_results', 'mic_results', 'result_cum', 'results_', 'x_array'],
output_keys=['counter_out','acc_results', 'cam_results', 'odom_results', 'imu_results', 'lidar_results', 'mic_results', 'result_cum', 'results_', 'x_array'])
#rospy.spin()
self.step = step
#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.imu_client = Client("imu_collision_detection", 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.is_first_time = True
rospy.sleep(0.2)
def callback(self,config):
#print (config)
pass
#rospy.loginfo("Config set to {double_param}, {int_param}, {double_param}, ".format(**config))
def execute(self, userdata):
rospy.loginfo('Executing SETUP')
#self.acc_client.update_configuration({"window_size": 10})#userdata.counter_in})
#self.acc_client.update_configuration({"reset": True})
#self.odom_client.update_configuration({"window_size": 10})#userdata.counter_in})
#self.odom_client.update_configuration({"reset": True})
self.imu_client.update_configuration({"window_size": 10})#userdata.counter_in})
#self.imu_client.update_configuration({"reset": True})
self.lidar_client.update_configuration({"window_size": 10})#userdata.counter_in})
#self.lidar_client.update_configuration({"reset": True})
self.mic_client.update_configuration({"window_size": 10})
#self.mic_client.update_configuration({"reset": True})
#SURF Version
#self.cam_client.update_configuration({"matching_threshold": 100})
self.cam_client.update_configuration({"mode": 0 })
#self.cam_client.update_configuration({"reset": True})
rospy.sleep(0.5)
if self.is_first_time:#userdata.counter_in < 75: # Window SIZe Define max TODO
userdata.x_array.append(userdata.counter_in)
userdata.counter_out = userdata.counter_in + self.step
self.is_first_time = False
return 'SETUP_DONE'
else:
userdata.results_['accel'] = userdata.acc_results
userdata.results_['cam1'] = userdata.cam_results
userdata.results_['odom'] = userdata.odom_results
userdata.results_['imu'] = userdata.imu_results
userdata.results_['lidar'] = userdata.lidar_results
userdata.results_['mic'] = userdata.mic_results
return 'FINISH'
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):
# 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 init_extended(self):
self.cpp_process_service = rospy.get_param("~cpp_service","cpp_process")
self.multi_exposure = rospy.get_param("~multi_exposure",False)
self.ignore_first_picture = True
if self.multi_exposure:
self.client = ReconfigureClient("prosilica_driver")
self.client.update_configuration({'exposure':0.4})
def load(self, name, path):
if not os.path.exists(path):
return False
if not name.startswith('/'):
name = '/' + name
f = file(path, 'r')
try:
params = {}
for doc in yaml.load_all(f.read(), Loader=yaml.FullLoader):
params.update(doc)
finally:
f.close()
try:
self.client = DynamicReconfigureClient(name)
try:
self.client.update_configuration(params)
except DynamicReconfigureCallbackException as err:
logger.log_error(str(err))
return False
except DynamicReconfigureParameterException as err:
logger.log_error(str(err))
return False
return True
finally:
self.client.close()
def new_uut(self):
"""
Deletes old Unit under Test, UUT, and creates a new one. Also resets
messages and config
:return: uut
:rtype: DelegationManager
"""
if hasattr(self, 'uut'):
self.uut.__del__()
self.uut = None
rospy.sleep(0.5)
uut = DelegationManager(name=self.uut_name)
self.mocked_DM.reset_messages()
self.uut = uut
self.dynrecClient = None
while not self.dynrecClient:
try:
self.dynrecClient = DynRecClient(
"/UUT/delegation_module_parameters", timeout=1)
except rospy.ROSInterruptException:
pass
rospy.sleep(0.3)
rospy.logwarn(
"NO DynRecClient created yet, waiting for dynamic_reconfigure services."
)
return uut
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")
class Client:
def __init__(self, name, timeout=None):
self.name = name
self.client = DynamicReconfigureClient(name, timeout)
def update(self, config):
# send data out no greater than 10 times
# revisit this and determine if this is actually needed
c = 0
new_config = {}
while c < 10:
# fuerte: update_configuration returns the configuration in a different format
# get new_config and compare with sent out config
updated_config = self.client.update_configuration(config)
for key,val in config.iteritems():
new_config[key] = updated_config[key]
c += 1
if new_config == config:
break
if c == 10:
pre = 'failed to set '
else:
pre = 'successfully set '
sleep_time = 0.05
rospy.loginfo(pre + self.name + ' in ' + str(c) + ' attempt(s) and sleeping for ' + str(sleep_time) + 's')
time.sleep(sleep_time)
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_extended(self):
self.cpp_process_service = rospy.get_param("~cpp_service",
"cpp_process")
self.multi_exposure = rospy.get_param("~multi_exposure", False)
self.ignore_first_picture = True
if self.multi_exposure:
self.client = ReconfigureClient("prosilica_driver")
self.client.update_configuration({'exposure': 0.4})
def __init__(self, sim=False):
super(MyStateMachine, self).__init__(sim)
StateMachine.__init__(self)
dsrv = DynamicReconfigureServer(RobotConfig, self.callback)
self.dclient = DynamicReconfigureClient("core",
timeout=30,
config_callback=None)
self.mir = MIR(HOST)
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 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())
class IntegrationTime:
MIN = 30
MAX = 2000
def __init__(self):
self.client = Client('camera/driver', timeout=1)
def set(self, time):
time = min(self.MAX, max(self.MIN, time))
self.client.update_configuration({'integration_time': time})
def get(self):
return self.client.get_configuration()['integration_time']
def add(self, time):
current = self.get()
self.set(current + time)
def __init__(self):
super(Explore, self).__init__(outcomes = ['succeeded', 'failed'], input_keys =['speed'])
self._action_topic = '/move_base'
self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})
self._dynrec = Client("/vehicle_controller", timeout = 10)
self._defaultspeed = 0.1
self._succeeded = False
self._failed = False
def __init__(self, name):
# Variables
self._action_name = name
self.display_no = rospy.get_param("~display", 0)
self.start_time = 0
self.route = TopologicalRoute()
self.coordinates = PoseArray()
self.direction = "straight"
self.previous_direction = ""
self.head_pub = rospy.Publisher('/head/commanded_state',
JointState,
queue_size=10)
self.head = JointState()
self.head.header.stamp = rospy.Time.now()
self.head.name = ["HeadPan", "HeadTilt"]
self.topological_nodes = rospy.wait_for_message(
'/topological_map', TopologicalMap)
self.route_sub = rospy.Subscriber("/topological_navigation/Route",
TopologicalRoute,
self.route_callback)
self.path_sub = message_filters.Subscriber("/move_base/NavfnROS/plan",
Path)
self.pose_sub = message_filters.Subscriber("/pose_extractor/pose",
PoseStamped)
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.path_sub, self.pose_sub], 10, 10)
self.ts.registerCallback(self.filter_callback)
self.dyn_client = DynClient('/EBC')
self.thread = None
#tell the webserver where it should look for web files to serve
#http_root = os.path.join(
# roslib.packages.get_pkg_dir("aaf_walking_group"),
# "www")
#client_utils.set_http_root(http_root)
#Starting server
rospy.loginfo("%s: Starting walking interface action server", name)
self._as = actionlib.SimpleActionServer(
self._action_name,
EmptyAction,
execute_cb=self.executeCallback,
auto_start=False)
self._as.start()
rospy.loginfo("%s: ...done.", name)
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__(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__(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()
class SpeechRecognitionClient(object):
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)
@property
def language(self):
return self._cfg.config["language"]
@language.setter
def language(self, value):
self._cfg.update_configuration({'language': value})
@property
def engine(self):
return self._cfg.config["engine"]
@engine.setter
def engine(self, value):
self._cfg.update_configuration({'engine': value})
@property
def energy_threshold(self):
return self._cfg.config["energy_threshold"]
@energy_threshold.setter
def energy_threshold(self, value):
if self.dynamic_energy_threshold:
rospy.logerr("Dynamic energy thresholding is enabled")
else:
self._cfg.update_configuration({
"energy_threshold": value
})
@property
def dynamic_energy_threshold(self):
return self._cfg.config["dynamic_energy_threshold"]
@dynamic_energy_threshold.setter
def dynamic_energy_threshold(self, value):
self._cfg.update_configuration({
"dynamic_energy_threshold": value
})
def recognize(self, **args):
return self._sr_srv(**args).result
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/DWAPlannerROS",
timeout=1.0)
changes = dict()
changes['acc_lim_x'] = maximum_f(
self.valid_config.accel_limit_mps2,
self.valid_config.decel_limit_mps2)
changes['acc_lim_y'] = maximum_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.x_vel_limit_mps
changes['max_vel_y'] = self.valid_config.y_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(
"Movo Driver updated move_base parameters to match machine parameters"
)
def async_apply_update(self, reconfig_request):
try:
#print "**** Start", self.name
if self.reconfigure_client == None:
#print "**** Making client", self.name
self.reconfigure_client = DynamicReconfigureClient(self.name)
#print "**** Made client", self.name
self.reconfigure_client.update_configuration(reconfig_request)
#print "**** Reconfigured client", self.name
#print "Done updating camera ", self.name
except KeyboardInterrupt: # Handle CTRL+C
print "Aborted camera update on", self.name
def async_apply_update(self, reconfig_request):
try:
#print "**** Start", self.name
if self.reconfigure_client == None:
#print "**** Making client", self.name
self.reconfigure_client = DynamicReconfigureClient(self.name)
#print "**** Made client", self.name
self.reconfigure_client.update_configuration(reconfig_request)
#print "**** Reconfigured client", self.name
#print "Done updating camera ", self.name
except KeyboardInterrupt: # Handle CTRL+C
print "Aborted camera update on", self.name
def __init__(self, allow_backwards = False):
'''
Constructor
'''
super(MoveToFixedWaypoint, self).__init__(outcomes=['reached', 'failed'],
input_keys=['waypoint','speed'])
self._action_topic = '/move_base'
self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})
#self.set_tolerance = rospy.ServiceProxy('/controller/set_alternative_tolerances', SetAlternativeTolerance)
self._dynrec = Client("/vehicle_controller", timeout = 10)
self._defaultspeed = 0.1
self._allow_backwards = allow_backwards
self._failed = False
self._reached = False
class Explore(EventState):
'''
Starts the Exploration Task via /move_base
># speed Speed of the robot
<= succeeded Exploration Task was successful
<= failed Exploration Task failed
'''
def __init__(self):
super(Explore, self).__init__(outcomes = ['succeeded', 'failed'], input_keys =['speed'])
self._action_topic = '/move_base'
self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})
self._dynrec = Client("/vehicle_controller", timeout = 10)
self._defaultspeed = 0.1
self._succeeded = False
self._failed = False
def execute(self, userdata):
if self._move_client.has_result(self._action_topic):
result = self._move_client.get_result(self._action_topic)
self._dynrec.update_configuration({'speed':self._defaultspeed})
if result.result == 1:
self._reached = True
Logger.loginfo('Exploration succeeded')
return 'succeeded'
else:
self._failed = True
Logger.logwarn('Exploration failed!')
return 'failed'
def on_enter(self, userdata):
speedValue = self._dynrec.get_configuration(timeout = 0.5)
if speedValue is not None:
self._defaultspeed = speedValue['speed']
self._dynrec.update_configuration({'speed':userdata.speed})
self._succeeded = False
self._failed = False
action_goal = MoveBaseGoal()
action_goal.exploration = True
action_goal.speed = userdata.speed
if action_goal.target_pose.header.frame_id == "":
action_goal.target_pose.header.frame_id = "world"
try:
if self._move_client.is_active(self._action_topic):
self._move_client.cancel(self._action_topic)
self._move_client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to start Exploration' % {
'err': str(e),
'x': userdata.waypoint.pose.position.x,
'y': userdata.waypoint.pose.position.y
})
self._failed = True
def on_exit(self, userdata):
self._move_client.cancel(self._action_topic)
def on_start(self):
pass
def on_stop(self):
pass
def configure_prosilica():
print "Configuring prosilica"
client = ReconfigureClient("prosilica_driver")
client.update_configuration ({'auto_exposure':False,'exposure':0.4,'auto_gain':False,'gain':0.0,
'auto_whitebalance':False,'whitebalance_red':114,'whitebalance_blue':300})
class CPPBridgeNode(ImageProcessor):
def init_extended(self):
self.cpp_process_service = rospy.get_param("~cpp_service","cpp_process")
self.multi_exposure = rospy.get_param("~multi_exposure",False)
self.ignore_first_picture = True
if self.multi_exposure:
self.client = ReconfigureClient("prosilica_driver")
self.client.update_configuration({'exposure':0.4})
def get_second_image(self,image_topic):
# Set exposure
print "Changing the exposure of camera to 0.2"
self.client.update_configuration({'exposure':0.2})
rospy.sleep(0.1)
image1 = TopicUtils.get_next_message(image_topic,Image)
print "Changing the exposure of camera to 0.4"
self.client.update_configuration({'exposure':0.4})
rospy.sleep(0.1)
image2 = TopicUtils.get_next_message(image_topic,Image)
return (image1,image2)
def process(self,cv_image,info,cv_image2=None):
#ignore and get my own pictures
if self.multi_exposure:
(image,image2) = self.get_second_image("prosilica/image_rect_color")
else:
image = self.bridge.cv_to_imgmsg(cv_image,"bgr8")
image2 = self.bridge.cv_to_imgmsg(cv_image,"bgr8")
# save and display image in order to document the current experiment
cvImage1= self.bridge.imgmsg_to_cv(image, "bgr8")
cvImage2= self.bridge.imgmsg_to_cv(image2, "bgr8")
timetag= strftime("%Y_%m_%d__%H_%M_%S")
imageName1= "/tmp/" + timetag + "_exposure_image1.png"
imageName2= "/tmp/" + timetag + "_exposure_image2.png"
print "======= Storing images of 2 exposures ======="
print "Saving image1: " + imageName1
cv.SaveImage(imageName1, cvImage1);
print "Saving image2: " + imageName2
cv.SaveImage(imageName2, cvImage2);
try:
cpp_process = rospy.ServiceProxy(self.cpp_process_service,ProcessBridge)
resp = cpp_process(image,info,image2)
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)
return ([],{},cv_image)
pts_y = resp.pts_y
pts_x = resp.pts_x
pts2d = []
for i in range(len(pts_y)):
x = pts_x[i]
y = pts_y[i]
pts2d.append((x,y))
params_list = resp.params
params = {}
names = ("l","r")
for i,val in enumerate(params_list):
params[names[i]] = val
image_annotated = resp.image_annotated
try:
cv_image_annotated = self.bridge.imgmsg_to_cv(image_annotated, "bgr8")
except CvBridgeError, e:
"CVERROR converting from cv IplImage to ImageMessage"
global old_callback
print("New callback is calling old callback...")
old_callback(config)
print("New callback is done...")
print('')
# First you need to connect to the server. You can optionally specify a
# timeout and a config_callback that is called each time the server's
# configuration changes. If you do not indicate a timeout, the client is
# willing to wait forever for the server to be available.
#
# Note that the config_callback could get called before the constructor
# returns.
rospy.init_node('testclient_py', anonymous=True)
client = DynamicReconfigureClient('/dynamic_reconfigure_test_server', config_callback=config_callback, timeout=5)
time.sleep(1)
# You can also get the configuration manually by calling get_configuration.
print("Configuration from get_configuration:")
print_config(client.get_configuration(timeout=5))
time.sleep(1)
# You can push changes to the server using the update_configuration method.
# You can set any subset of the node's parameters using this method. It
# returns out the full new configuration of the server (which may differ
# from what you requested if you asked for something illegal).
print("Configuration after setting int_ to 4:")
print_config(client.update_configuration({'int_': 4}))
time.sleep(1)
class MoveToFixedWaypoint(EventState):
'''
Lets the robot move to a given waypoint.
-- allow_backwards Boolean Allow the robot to drive backwards when approaching the given waypoint.
># waypoint PoseStamped Specifies the waypoint to which the robot should move.
># speed Speed of the robot
<= reached Robot is now located at the specified waypoint.
<= failed Failed to send a motion request to the action server.
'''
def __init__(self, allow_backwards = False):
'''
Constructor
'''
super(MoveToFixedWaypoint, self).__init__(outcomes=['reached', 'failed'],
input_keys=['waypoint','speed'])
self._action_topic = '/move_base'
self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})
#self.set_tolerance = rospy.ServiceProxy('/controller/set_alternative_tolerances', SetAlternativeTolerance)
self._dynrec = Client("/vehicle_controller", timeout = 10)
self._defaultspeed = 0.1
self._allow_backwards = allow_backwards
self._failed = False
self._reached = False
def execute(self, userdata):
'''
Execute this state
'''
if self._failed:
return 'failed'
if self._reached:
return 'reached'
if self._move_client.has_result(self._action_topic):
result = self._move_client.get_result(self._action_topic)
self._dynrec.update_configuration({'speed':self._defaultspeed})
if result.result == 1:
self._reached = True
return 'reached'
else:
self._failed = True
Logger.logwarn('Failed to reach waypoint!')
return 'failed'
def on_enter(self, userdata):
speedValue = self._dynrec.get_configuration(timeout = 0.5)
if speedValue is not None:
self._defaultspeed = speedValue['speed']
self._dynrec.update_configuration({'speed':userdata.speed})
self._startTime = Time.now()
self._failed = False
self._reached = False
#goal_id = GoalID()
#goal_id.id = 'abcd'
#goal_id.stamp = Time.now()
action_goal = MoveBaseGoal()
action_goal.target_pose = userdata.waypoint
action_goal.speed = userdata.speed
action_goal.reverse_allowed = self._allow_backwards
if action_goal.target_pose.header.frame_id == "":
action_goal.target_pose.header.frame_id = "world"
try:
self._move_client.send_goal(self._action_topic, action_goal)
#resp = self.set_tolerance(goal_id, 0.2, 1.55)
except Exception as e:
Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % {
'err': str(e),
'x': userdata.waypoint.pose.position.x,
'y': userdata.waypoint.pose.position.y
})
self._failed = True
Logger.loginfo('Driving to next waypoint')
def on_stop(self):
try:
if self._move_client.is_available(self._action_topic) \
and not self._move_client.has_result(self._action_topic):
self._move_client.cancel(self._action_topic)
except:
# client already closed
pass
def on_exit(self, userdata):
try:
if self._move_client.is_available(self._action_topic) \
and not self._move_client.has_result(self._action_topic):
self._move_client.cancel(self._action_topic)
except:
# client already closed
pass
def on_pause(self):
self._move_client.cancel(self._action_topic)
def on_resume(self, userdata):
self.on_enter(userdata)
class Camera:
def __init__(self, node_name, proj, level, **paramnames):
self.paramnames = paramnames
self.name = node_name
self.level = level
self.proj = proj
self.reconfigure_client = None
self.async = AsynchronousUpdater(self.async_apply_update, "Camera "+node_name)
self.trig_rising = True
def param(self, config, name):
return config[self.paramnames[name]]
def setparam(self, config, name, value):
config[self.paramnames[name]] = value
# Uses the new configuration to compute new camera state.
def process_update(self, config, level):
# Produces:
# MEMBERS
# period, ext_trig, imager_period, end_offset
#
# PARAMETERS
# "param_rate"
self.trigger_name = "not_set" # Will be set by the CameraTriggerController
# Took level checking out as it was causing problems with the projector
# needed flag.
#if self.level & level == 0:
# return # This camera's configuration is unchanged.
self.period = 1.0 / self.param(config, param_rate)
projector_rate = config[param_proj_rate]
self.reset_cameras = config["camera_reset"]
self.ext_trig = True
self.register_set = WGEConfig.WGE100Camera_PrimaryRegisterSet
projector_limits_exposure = True
trig_mode = self.param(config, param_trig_mode)
# Internal triggering.
if trig_mode == Config.CameraSynchronizer_InternalTrigger:
self.ext_trig = False
self.imager_period = self.period
self.end_offset = -1
projector_limits_exposure = False
else:
if self.proj.mode == Config.CameraSynchronizer_ProjectorOff:
trig_mode = Config.CameraSynchronizer_IgnoreProjector
if trig_mode == Config.CameraSynchronizer_AlternateProjector or trig_mode == Config.CameraSynchronizer_WithProjector:
self.proj.needed = True
#print self.name, "setting needed to true."
#else:
#print self.name, "not setting needed to true."
# Set the period
if trig_mode == Config.CameraSynchronizer_AlternateProjector:
n = round(self.period / self.proj.repeat_period - 0.5)
n = max(n, 0)
self.period = (n + 0.5) * self.proj.repeat_period
#print "Case 1", n
elif trig_mode == Config.CameraSynchronizer_IgnoreProjector:
self.period = roundToEthercat(self.period)
else:
n = round(2 * self.period / self.proj.repeat_period - 1)
n = max(n, 0)
self.period = (n + 1) * self.proj.repeat_period / 2.0
#print "Case 2", n, self.period
# Set the end_offset
if trig_mode == Config.CameraSynchronizer_IgnoreProjector:
self.end_offset = 0
projector_limits_exposure = False
if trig_mode == Config.CameraSynchronizer_AlternateProjector:
self.end_offset = self.proj.alt_end_offset
self.register_set = WGEConfig.WGE100Camera_Auto
elif trig_mode == Config.CameraSynchronizer_WithProjector:
self.end_offset = self.proj.proj_end_offset
self.register_set = WGEConfig.WGE100Camera_AlternateRegisterSet
else:
self.end_offset = self.proj.noproj_end_offset
# Pick the imager period
if trig_mode == Config.CameraSynchronizer_IgnoreProjector:
self.imager_period = self.period - ETHERCAT_INTERVAL
else:
self.imager_period = self.proj.repeat_period / 2 - ETHERCAT_INTERVAL
#print "Camera period", self.name, self.period, self.imager_period, self.proj.repeat_period
if projector_limits_exposure:
self.max_exposure = self.proj.noproj_max_exposure
#print "Exposurei projector", self.max_exposure
else:
self.max_exposure = self.imager_period * 0.95
#print "Exposurei imager", self.max_exposure
self.setparam(config, param_rate, 1/self.period)
self.setparam(config, param_trig_mode, trig_mode)
def async_apply_update(self, reconfig_request):
try:
#print "**** Start", self.name
if self.reconfigure_client == None:
#print "**** Making client", self.name
self.reconfigure_client = DynamicReconfigureClient(self.name)
#print "**** Made client", self.name
self.reconfigure_client.update_configuration(reconfig_request)
#print "**** Reconfigured client", self.name
#print "Done updating camera ", self.name
except KeyboardInterrupt: # Handle CTRL+C
print "Aborted camera update on", self.name
def apply_update(self):
reconfig_request = {
"ext_trig" : self.ext_trig,
"trig_rate" : 1.0 / self.period,
"imager_rate" : 1.0 / self.imager_period,
"trig_timestamp_topic" : self.trigger_name,
"rising_edge_trig" : self.trig_rising,
"register_set" : self.register_set,
"auto_exposure_alternate" : False,
"exposure_alternate" : self.proj.proj_exposure_duration,
"max_exposure" : self.max_exposure,
}
#print self.name, reconfig_request
self.async.update(reconfig_request)