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 _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 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 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 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()
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()
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_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()
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_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()
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 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_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()
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 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()
class DriveTeleop:
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 param_server_callback(self, config):
self.drive_state = config['drive_state']
self.speed_setting = config['speed_setting']
rospy.loginfo(config)
def on_joy(self, data):
if data.buttons[6] == 1:
self.drive_state = 1 - self.drive_state
self.clt.update_configuration({"drive_state": self.drive_state})
if self.drive_state == 1:
# Set speed ratio using d-pad
if data.axes[7] == 1: # full speed (d-pad up)
self.speed_setting = 1
self.clt.update_configuration({"speed_setting": 1})
if data.axes[6] != 0: # medium speed (d-pad left or right)
self.speed_setting = 2
self.clt.update_configuration({"speed_setting": 2})
if data.axes[7] == -1: # low speed (d-pad down)
self.speed_setting = 3
self.clt.update_configuration({"speed_setting": 3})
# Drive sticks
left_speed = data.axes[1] / self.speed_setting # left stick
right_speed = data.axes[4] / self.speed_setting # right stick
# Convert skid steering speeds to twist speeds
linear_vel = (left_speed + right_speed) / 2.0 # (m/s)
angular_vel = (right_speed - left_speed) / 2.0 # (rad/s)
# Publish Twist
twist = Twist()
twist.linear.x = linear_vel
twist.angular.z = angular_vel
self.cmd_vel_pub.publish(twist)
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
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"
class DynamicVelocityReconfigure():
"A class to reconfigure the velocity of the DWAPlannerROS."
GAZE, NO_GAZE = range(2)
def __init__(self, name):
rospy.loginfo("Starting %s", name)
self._action_name = name
self.fast = True
self.gaze_type = DynamicVelocityReconfigure.GAZE
rospy.loginfo("Creating dynamic reconfigure client")
self.client = DynClient("/move_base/DWAPlannerROS")
rospy.loginfo(" ...done")
rospy.loginfo("Creating base movement client.")
self.baseClient = actionlib.SimpleActionClient(
'move_base', move_base_msgs.msg.MoveBaseAction)
self.baseClient.wait_for_server()
rospy.loginfo("Creating gaze client.")
self.gazeClient = actionlib.SimpleActionClient(
'gaze_at_pose', strands_gazing.msg.GazeAtPoseAction)
self.gazeClient.wait_for_server()
rospy.loginfo("...done")
# Magic numbers overwritten in dyn_callback
self.threshold = 4.0
self.max_dist = 5.0
self.min_dist = 1.5
self.detection_angle = 90.0
self.dyn_srv = DynServer(HumanAwareNavigationConfig, self.dyn_callback)
current_time = rospy.get_time()
self.timeout = current_time + self.threshold
rospy.loginfo("Creating action server.")
self._as = actionlib.SimpleActionServer(
self._action_name,
move_base_msgs.msg.MoveBaseAction,
self.goalCallback,
auto_start=False)
rospy.loginfo(" ...starting")
self._as.start()
rospy.loginfo(" ...done")
self.sub_topic = rospy.get_param("~people_positions",
'/people_tracker/positions')
self.ppl_sub = None
self.last_cancel_time = rospy.Time(0)
rospy.Subscriber("/human_aware_navigation/cancel",
actionlib_msgs.msg.GoalID,
self.cancel_time_checker_cb)
def dyn_callback(self, config, level):
if config["gaze_type"] == DynamicVelocityReconfigure.NO_GAZE:
self.cancel_gaze_goal()
self.gaze_type = config["gaze_type"]
self.threshold = config["timeout"]
self.max_dist = config["max_dist"]
self.min_dist = config["min_dist"]
self.detection_angle = config["detection_angle"]
max_vel_x = config["max_vel_x"]
max_trans_vel = config["max_trans_vel"]
max_rot_vel = config["max_rot_vel"]
self.fast_param = {
"max_vel_x": max_vel_x,
"max_trans_vel": max_trans_vel,
"max_rot_vel": max_rot_vel
}
return config
def cancel_time_checker_cb(self, msg):
self.last_cancel_time = rospy.get_rostime()
def send_gaze_goal(self, topic):
if self.gaze_type == DynamicVelocityReconfigure.GAZE:
gaze_goal = strands_gazing.msg.GazeAtPoseGoal()
gaze_goal.runtime_sec = 0
gaze_goal.topic_name = topic
self.gazeClient.send_goal(gaze_goal)
def cancel_gaze_goal(self):
if self.gaze_type == DynamicVelocityReconfigure.GAZE:
self.gazeClient.cancel_all_goals()
def resetSpeed(self):
rospy.logdebug("Resetting speeds to max:")
rospy.logdebug(" Setting parameters: %s", self.fast_param)
try:
self.client.update_configuration(self.fast_param)
except rospy.ServiceException as exc:
rospy.logerr("Caught service exception: %s", exc)
try:
self.client.update_configuration(self.fast_param)
except rospy.ServiceException as exc:
rospy.logerr("Caught service exception: %s", exc)
try:
self.client.update_configuration(self.fast_param)
except rospy.ServiceException as exc:
rospy.logerr("Caught service exception: %s", exc)
self.baseClient.cancel_all_goals()
self.cancel_gaze_goal()
self._as.set_aborted()
def get_min_dist(self, data, angle):
rad = angle * (np.pi / 180.0)
distances = [
x for idx, x in enumerate(data.distances)
if data.angles[idx] >= -rad and data.angles[idx] <= rad
]
return np.min(distances) if len(distances) else 1000.0
def pedestrianCallback(self, pl):
if not self._as.is_active():
rospy.logdebug("No active goal. Unsubscribing.")
if self.ppl_sub:
self.ppl_sub.unregister()
self.ppl_sub = None
return
if len(pl.poses) > 0:
rospy.logdebug("Found people: ")
min_distance = self.get_min_dist(pl, self.detection_angle)
rospy.logdebug(" People distance: %s", min_distance)
factor = min_distance - self.min_dist
factor = factor if factor > 0.0 else 0.0
factor /= (self.max_dist - self.min_dist)
factor = 1.0 if factor > 1.0 else factor
trans_speed = factor * self.fast_param['max_vel_x']
trans_speed = round(trans_speed, 2)
rospy.logdebug("Calculated translational speed: %s", trans_speed)
rot_speed = factor * self.fast_param['max_rot_vel']
rot_speed = round(rot_speed, 2)
rospy.logdebug("Calculated rotaional speed: %s", rot_speed)
if not trans_speed == self.fast_param[
'max_vel_x']: # and not rot_speed == self.fast_param['max_rot_vel']:
self.send_gaze_goal(
"/upper_body_detector/closest_bounding_box_centre")
self.slow_param = {
'max_vel_x':
trans_speed,
'max_trans_vel':
trans_speed,
'max_rot_vel':
0.0
if trans_speed < 0.05 else self.fast_param["max_rot_vel"]
}
try:
print 'making it slow'
self.client.update_configuration(self.slow_param)
except rospy.ServiceException as exc:
rospy.logerr("Caught service exception: %s", exc)
rospy.logdebug(" Setting parameters: %s", self.slow_param)
self.fast = False
self.timeout = rospy.get_time() + self.threshold
elif rospy.get_time() > self.timeout:
rospy.logdebug("Not found any pedestrians:")
if not self.fast:
self.send_gaze_goal("/pose_extractor/pose")
self.resetSpeed()
self.fast = True
else:
rospy.logdebug(" Already fast")
def goalCallback(self, goal):
self.ppl_sub = rospy.Subscriber(self.sub_topic, PeopleTracker,
self.pedestrianCallback, None, 1)
self._goal = goal
self.send_gaze_goal("/pose_extractor/pose")
rospy.logdebug("Received goal:\n%s", self._goal)
self.resetSpeed()
self.moveBaseThread(self._goal)
def preemptCallback(self):
if rospy.get_rostime() - self.last_cancel_time < rospy.Duration(1):
rospy.logdebug("Cancelled execution of goal:\n%s", self._goal)
self.baseClient.cancel_all_goals()
self.cancel_gaze_goal()
self.resetSpeed()
self._as.set_preempted()
def moveBaseThread(self, goal):
ret = self.moveBase(goal)
self.resetSpeed()
self.cancel_gaze_goal()
if not self._as.is_preempt_requested() and ret:
self._as.set_succeeded(self.result)
elif not self._as.is_preempt_requested() and not ret:
self._as.set_aborted(self.result)
def moveBase(self, goal):
rospy.logdebug('Moving robot to goal: %s', goal)
self.baseClient.send_goal(goal,
feedback_cb=self.moveBaseFeedbackCallback)
status = self.baseClient.get_state()
while status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.ACTIVE:
status = self.baseClient.get_state()
self.baseClient.wait_for_result(rospy.Duration(0.2))
self.result = self.baseClient.get_result()
if self._as.is_preempt_requested():
self.preemptCallback()
break
if self.baseClient.get_state(
) != actionlib_msgs.msg.GoalStatus.SUCCEEDED and self.baseClient.get_state(
) != actionlib_msgs.msg.GoalStatus.PREEMPTED:
return False
#avoid jumping out of a state immediately after entering it - actionlib bug
rospy.sleep(rospy.Duration.from_sec(0.3))
return True
def moveBaseFeedbackCallback(self, fb):
self._as.publish_feedback(fb)
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 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)
class WalkingInterfaceServer(object):
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 filter_callback(self, path, pose):
self.start_time = time.time()
robot_angle = tf.transformations.euler_from_quaternion([
pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w
])
yDiff = path.poses[-1].pose.position.y - pose.pose.position.y
xDiff = path.poses[-1].pose.position.x - pose.pose.position.x
dist = math.sqrt(xDiff * xDiff + yDiff * yDiff)
if dist > 1.0:
angle = robot_angle[-1] - math.atan2(yDiff, xDiff)
while angle > math.pi:
angle -= 2 * math.pi
while angle < -math.pi:
angle += 2 * math.pi
#print math.degrees(angle)
if abs(math.degrees(angle)) < 30:
self.direction = "straight"
else:
if angle > 0:
self.direction = "right"
else:
self.direction = "left"
else:
self.direction = "straight"
def route_callback(self, data):
#search for waypouint and store coordinates
print data.nodes
for i in data.nodes:
for j in self.topological_nodes.nodes:
if i == j.name:
self.coordinates.poses.append(j.pose)
def executeCallback(self, goal):
self.previous_direction = ""
while not self._as.is_preempt_requested() and not rospy.is_shutdown():
if time.time() - self.start_time > 5: #reconfigurable parameter
self.direction = "stop"
if self.previous_direction != self.direction:
client_utils.display_relative_page(self.display_no,
'stop.html')
rospy.loginfo("STOP")
self.indicate = False
#move head straight
self.head.position = [0, 0]
self.head_pub.publish(self.head)
self.previous_direction = "stop"
else:
if self.previous_direction != self.direction:
if self.direction == 'right':
client_utils.display_relative_page(
self.display_no, 'turn_right.html')
#move head right
self.head.position = [-30, 0]
self.head_pub.publish(self.head)
#indicate
if self.thread:
self.indicate = False
self.thread.join()
self.indicate = True
self.thread = Thread(target=self.blink,
args=('right', ))
self.thread.start()
rospy.loginfo("Moving right...")
self.previous_direction = "right"
elif self.direction == 'left':
client_utils.display_relative_page(
self.display_no, 'turn_left.html')
#move head left
self.head.position = [30, 0]
self.head_pub.publish(self.head)
#indicate
if self.thread:
self.indicate = False
self.thread.join()
self.indicate = True
self.thread = Thread(target=self.blink,
args=('left', ))
self.thread.start()
rospy.loginfo("Moving left...")
self.previous_direction = "left"
else:
client_utils.display_relative_page(
self.display_no, 'straight.html')
self.indicate = False
#move head straight
self.head.position = [0, 0]
self.head_pub.publish(self.head)
rospy.loginfo("Moving straight...")
self.previous_direction = "straight"
if self._as.is_preempt_requested():
self._as.set_preempted()
else:
self._as.set_succeeded()
def switchIndicator(self, isOn, side):
if side == "left":
params = {'Port0_5V_Enabled': isOn}
else:
params = {'Port1_5V_Enabled': isOn}
self.dyn_client.update_configuration(params)
def blink(self, side):
r = rospy.Rate(2)
toggle = True
while self.indicate and not rospy.is_shutdown():
self.switchIndicator(toggle, side)
toggle = not toggle
r.sleep()
self.switchIndicator(False, side)
def _on_node_shutdown(self):
self.client.cancel_all_goals()
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"
class WalkingGroupStateMachine(object):
def __init__(self, name):
rospy.loginfo("Creating " + name + " server...")
self._as = actionlib.SimpleActionServer(
name,
StateMachineAction,
self.execute,
auto_start=False
)
self._as.register_preempt_callback(self.preempt_callback)
rospy.loginfo("Creating guiding client...")
nav_client = actionlib.SimpleActionClient("guiding", GuidingAction)
nav_client.wait_for_server()
rospy.loginfo(" ... done")
rospy.loginfo("Creating wait client...")
wait_client = actionlib.SimpleActionClient("wait_for_participant", EmptyAction)
wait_client.wait_for_server()
rospy.loginfo(" ... done")
rospy.loginfo("Waiting for recovery toggle services...")
rospy.loginfo(" ... images")
self.rec_srv = rospy.ServiceProxy('/toggle_walking_group_recovery', ToggleWalkingGroupRecovery)
self.rec_srv.wait_for_service()
rospy.loginfo(" ... done")
rospy.loginfo("Waiting for media server services...")
rospy.loginfo(" ... images")
s = rospy.ServiceProxy('/aaf_walking_group/image_server/get_id', GetMediaId)
s.wait_for_service()
rospy.loginfo(" ... video")
s = rospy.ServiceProxy('/aaf_walking_group/video_server/get_id', GetMediaId)
s.wait_for_service()
rospy.loginfo(" ... sound")
rospy.loginfo(" ... music")
s = rospy.ServiceProxy('/music_player_service', MusicPlayerService)
s.wait_for_service()
rospy.loginfo(" ... jingles")
s = rospy.ServiceProxy('/sound_player_service', PlaySoundService)
s.wait_for_service()
rospy.loginfo(" ... waypoints")
s = rospy.ServiceProxy('/aaf_waypoint_sounds_service', WaypointSoundsService)
s.wait_for_service()
rospy.loginfo(" ... done")
# Get parameters
self.display_no = rospy.get_param("~display_no", 0)
self.waypointset_name = rospy.get_param("~mongodb_params/waypointset_name", "")
self.waypointset_collection = rospy.get_param("~mongodb_params/waypointset_collection", "aaf_walking_group")
self.waypointset_meta = rospy.get_param("~mongodb_params/waypointset_meta", "waypoint_set")
if self.waypointset_name == "":
rospy.logfatal("Missing parameters.")
rospy.logfatal("Please run with _mongodb_params/waypointset_name:=<waypointset_name>")
return
self.preempt_srv = None
self.ptu = PTU()
self.gaze = Gaze()
self.recovery = RecoveryReconfigure(
name="/monitored_navigation/recover_states/",
whitelist=rosparam.load_file(rospy.get_param("~recovery_whitelist"))[0][0]["recover_states"]
)
self.dyn_client = DynClient(
"/human_aware_navigation"
)
self.get_current_han_settings()
rospy.loginfo(" ... starting " + name)
self._as.start()
rospy.loginfo(" ... started " + name)
def get_current_han_settings(self):
gazing = rospy.get_param("/human_aware_navigation/gaze_type")
angle = round(rospy.get_param("/human_aware_navigation/detection_angle"),2)
self.han_param = {
'gaze_type': gazing,
'detection_angle': angle
}
rospy.loginfo("Found following default values for human_aware_navigation: %s", self.han_param)
def execute(self, goal):
rospy.loginfo("Starting state machine")
try:
rospy.loginfo("Creating recovery toggle service proxy and waiting ...")
self.rec_srv.wait_for_service()
rospy.loginfo(" .. calling recovery toggle service")
self.rec_srv(True)
rospy.loginfo(" .. started walking group recovery")
except (rospy.ServiceException, rospy.ROSInterruptException) as e:
rospy.logwarn("waypoint sound service call failed: %s" % e)
#Save current recovery behaviour states
self.recovery.save_current_configuration()
#and reconfigure
self.recovery.reconfigure(RecoveryReconfigure.SET)
# Setting http root
http_root = roslib.packages.get_pkg_dir('aaf_walking_group') + '/www'
strands_webserver.client_utils.set_http_root(http_root)
self.ptu.turnPTU(-180, 10)
dyn_param = {
'timeout': 0.0,
'gaze_type': 1,
'detection_angle': 80.0
}
try:
self.dyn_client.update_configuration(dyn_param)
except rospy.ServiceException as e:
rospy.logerr("Caught service exception: %s", e)
self.preempt_srv = rospy.Service('/walking_group/cancel', Empty, self.preempt_srv_cb)
self.waypointset = self.loadConfig(self.waypointset_name, collection_name=self.waypointset_collection, meta_name=self.waypointset_meta)
pprint.pprint(self.waypointset)
try:
rospy.loginfo("Creating waypoint sound service proxy and waiting ...")
s = rospy.ServiceProxy('aaf_waypoint_sounds_service', WaypointSoundsService)
s.wait_for_service()
rospy.loginfo(" .. calling waypoint sound service")
s(WaypointSoundsServiceRequest.RESUME)
rospy.loginfo(" .. started waypoint sound service")
except (rospy.ServiceException, rospy.ROSInterruptException) as e:
rospy.logwarn("waypoint sound service call failed: %s" % e)
# Create a SMACH state machine
self.sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
self.sm.userdata.current_waypoint = self.waypointset[goal.group]["waypoints"][str(min([int(x) for x in self.waypointset[goal.group]["waypoints"].keys()]))]
self.sm.userdata.play_music = True
sis = smach_ros.IntrospectionServer(
'walking_group_state_machine',
self.sm,
'/walking_group_machine'
)
sis.start()
# Open the container
with self.sm:
# Add states to the container
smach.StateMachine.add(
'ENTERTAIN',
Entertain(self.display_no, self.gaze),
transitions={
'key_card': 'GUIDE_INTERFACE',
'killall': 'preempted'
},
remapping={'current_waypoint' : 'current_waypoint', 'play_music' : 'play_music'}
)
smach.StateMachine.add(
'GUIDE_INTERFACE',
GuideInterface(self.waypointset[goal.group]["waypoints"]),
transitions={
'move_to_point': 'GUIDING',
'aborted': 'ENTERTAIN',
'killall': 'preempted'
},
remapping={'current_waypoint' : 'current_waypoint', 'play_music' : 'play_music'}
)
smach.StateMachine.add(
'GUIDING',
Guiding(waypoints=self.waypointset[goal.group]["waypoints"], distance=self.waypointset[goal.group]["stopping_distance"]),
transitions={
'reached_point': 'RESTING_CONT',
'reached_final_point': 'succeeded',
'key_card': 'GUIDE_INTERFACE',
'killall': 'preempted'
},
remapping={'waypoint' : 'waypoint', 'play_music' : 'play_music'}
)
smach.StateMachine.add(
'RESTING_CONT',
RestingPoint(self.display_no,self.waypointset[goal.group]["waypoints"]),
transitions={
'rest': 'ENTERTAIN',
'continue': 'GUIDING',
'key_card': 'GUIDE_INTERFACE',
'killall': 'preempted'
},
remapping={'current_waypoint' : 'current_waypoint', 'play_music' : 'play_music'}
)
# Execute SMACH plan
self.sm.execute()
sis.stop()
self.preempt_srv.shutdown()
self.ptu.turnPTU(0, 0)
self.recovery.reconfigure(RecoveryReconfigure.RESET)
try:
self.dyn_client.update_configuration(self.han_param)
except rospy.ServiceException as e:
rospy.logerr("Caught service exception: %s", e)
try:
rospy.loginfo("Creating waypoint sound service proxy and waiting ...")
s = rospy.ServiceProxy('aaf_waypoint_sounds_service', WaypointSoundsService)
s.wait_for_service()
rospy.loginfo(" ... calling waypoint sound service")
s(WaypointSoundsServiceRequest.PAUSE)
rospy.loginfo(" ... stopped waypoint sound service")
except (rospy.ServiceException, rospy.ROSInterruptException) as e:
rospy.logwarn("Service call failed: %s" % e)
try:
rospy.loginfo("Creating recovery toggle service proxy and waiting ...")
self.rec_srv.wait_for_service()
rospy.loginfo(" .. calling recovery toggle service")
self.rec_srv(False)
rospy.loginfo(" .. stopped walking group recovery")
except (rospy.ServiceException, rospy.ROSInterruptException) as e:
rospy.logwarn("Service call failed: %s" % e)
if not self._as.is_preempt_requested() and self._as.is_active():
self._as.set_succeeded()
else:
self._as.set_preempted()
def preempt_callback(self):
rospy.logwarn("Walking group preempt requested")
self.sm.request_preempt()
def preempt_srv_cb(self, req):
self.preempt_callback()
return EmptyResponse()
def loadConfig(self, dataset_name, collection_name="aaf_walking_group", meta_name="waypoint_set"):
msg_store = MessageStoreProxy(collection=collection_name)
query_meta = {}
query_meta[meta_name] = dataset_name
if len(msg_store.query(std_msgs.msg.String._type, {}, query_meta)) == 0:
rospy.logerr("Desired data set '"+meta_name+": "+dataset_name+"' not in datacentre.")
raise Exception("Can't find data in datacentre.")
else:
message = msg_store.query(std_msgs.msg.String._type, {}, query_meta)
return json.loads(message[0][0].data)
class DynamicConfigurationParameters:
def __init__(self):
self.defaults = {}
for param_name in rospy.get_param_names():
if not param_name.startswith(rospy.get_name()):
continue
value = str(rospy.get_param(param_name))
index = value.find(';')
name = value[:index]
path = value[index+1:]
param_name = param_name.split('/')[2]
self.defaults[param_name] = {
'name': name,
'path': path,
}
rospy.Service('/mercury/dynparam/defaults',
Default, self.default_handler)
rospy.Service('/mercury/dynparam/load', Load, self.load_handler)
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 default_handler(self, request):
key = request.name
if key not in self.defaults:
return False
logger.log_warn("loading default parameters for %s ...", key)
return self.load(self.defaults[key]['name'], self.defaults[key]['path'])
def load_handler(self, request):
if request.name == "":
return False
return self.load(request.name, request.path)
def spin(self):
rospy.spin()
# 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)
print("Configuration after setting int_ to 0 and bool_ to True:")
print_config(client.update_configuration({'int_': 0, 'bool_': True}))
time.sleep(1)
# You can access constants defined in Test.cfg file in the following way:
import dynamic_reconfigure.cfg.TestConfig as Config
print("Medium is a constant that is set to 1:", Config.Test_Medium)
# This is useful for setting enums:
print("Configuration after setting int_enum_ to Medium:")
print_config(client.update_configuration({'int_enum_': Config.Test_Medium}))
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 DoUTurnAndGoToStop1Stop2ThenSAndHomeWhileStopping(smach.State):
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 _stop_sign_detected_cb(self, msg):
self.last_stop = msg.data
def ndt_pose_cb(self, msg):
self.last_pose = msg
def did_we_see_stop(self):
msg = rospy.wait_for_message('/stop_detection_status', String, 4.0)
if msg.data == 'STOP':
return True
else:
return False
def are_we_at_stop_1(self):
stop_1_x = -57.978
stop_1_y = 112.837
return self.are_we_at(stop_1_x, stop_1_y)
def are_we_at_stop_2(self):
stop_1_x = -53.440
stop_1_y = 57.763
return self.are_we_at(stop_1_x, stop_1_y)
def are_we_at(self, x, y, tolerance=0.5):
curr_x = self.last_pose.pose.position.x
curr_y = self.last_pose.pose.position.y
dist = math.hypot(curr_x - x, curr_y - y)
rospy.loginfo("We are at " + str(round(dist, 2)) + "m")
if abs(dist) < tolerance:
return True
else:
return False
def stop_car_3s(self):
ini_t = time.time()
rospy.logerr("======== STOPPING FOR 3S ===========")
self.twist_gain_dyn_client.update_configuration({
'linear_x_gain': 0.0,
'angular_z_gain': 0.0
})
while not rospy.is_shutdown() and (time.time() - ini_t) < 5.0:
rospy.sleep(0.05)
rospy.loginfo("Done!")
self.twist_gain_dyn_client.update_configuration({
'linear_x_gain': 1.0,
'angular_z_gain': 25.0
})
def execute(self, userdata):
rospy.loginfo('Executing state ' + self.__class__.__name__)
# Send a goal to our "Move using waypoints" server and wait until
# we reach the goal
fwf = FollowWaypointsFile('mission_5_6_7.csv',
consider_done_on_waypoint_id=489)
rospy.sleep(2.0)
# keep checking for our pose to be in the radius of the first stop signal
while not rospy.is_shutdown() and \
not self.are_we_at_stop_1():
rospy.sleep(0.1)
if self.did_we_see_stop():
# stop the car 3s
self.stop_car_3s()
# wait for the car to move enough to stop seeing the STOP sign and reset
rospy.sleep(10.0)
self.last_stop = 'GO'
# keep checking for our pose to be in the radius of the first stop signal
while not rospy.is_shutdown() and \
not self.are_we_at_stop_2():
rospy.sleep(0.1)
if self.did_we_see_stop():
# stop the car 3s
self.stop_car_3s()
fwf.wait_to_reach_last_waypoint()
return 'succeeded'
def people_cb2(msg):
global tx2_people
tx2_people = msg
if __name__ == '__main__':
rospy.init_node('visualize_openpose')
stage = rospy.get_param('~stage', 1)
compute_openpose_xavier = rospy.ServiceProxy('compute_openpose', Compute)
compute_openpose_xavier.wait_for_service()
reconf_xavier = ReconfClient('openpose_xavier')
reconf_xavier.update_configuration({
'key_point_threshold': 0.1,
'affinity_threshold': 0.1,
'line_division': 5
})
reconf_tx2 = ReconfClient('openpose_tx2')
reconf_tx2.update_configuration({
'key_point_threshold': 0.1,
'affinity_threshold': 0.1,
'line_division': 2
})
#st_sub = rospy.Subscriber('openpose_stage1', SparseTensorArray,
# sp_cb, queue_size=1)
rospy.Subscriber('image',
Image,
img_cb,
queue_size=1,
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)
print("Configuration after setting int_ to 0 and bool_ to True:")
print_config(client.update_configuration({'int_': 0, 'bool_': True}))
time.sleep(1)
# You can access constants defined in Test.cfg file in the following way:
import dynamic_reconfigure.cfg.TestConfig as Config
print("Medium is a constant that is set to 1:", Config.Test_Medium)
# This is useful for setting enums:
print("Configuration after setting int_enum_ to Medium:")
print_config(client.update_configuration({'int_enum_': Config.Test_Medium}))
time.sleep(1)
def __init__(self, robot, rack_id):
StateMachine.__init__(self, outcomes=['done'])
public_arm = robot.get_arm()
arm = public_arm._arm
local_client = Client("/hero/local_planner/local_costmap")
local_client.update_configuration({"footprint": []})
global_client = Client("/hero/global_planner/global_costmap")
global_client.update_configuration({"footprint": []})
def send_joint_goal(position_array, wait_for_motion_done=True):
arm._send_joint_trajectory([position_array],
timeout=rospy.Duration(0))
if wait_for_motion_done:
arm.wait_for_motion_done()
def send_gripper_goal(open_close_string, max_torque=1.0):
arm.send_gripper_goal(open_close_string, max_torque=max_torque)
rospy.sleep(1.0) # Does not work with motion_done apparently
def show_image(package_name, path_to_image_in_package):
path = os.path.join(rospkg.RosPack().get_path(package_name),
path_to_image_in_package)
if not os.path.exists(path):
rospy.logerr("Image path {} does not exist".format(path))
else:
try:
rospy.loginfo("Showing {}".format(path))
robot.hmi.show_image(path, 10)
except Exception as e:
rospy.logerr("Could not show image {}: {}".format(path, e))
return 'succeeded'
@cb_interface(outcomes=['done'])
def _pre_grab(_):
robot.speech.speak("Hey, I found myself a nice rack")
send_joint_goal([0, 0, 0, 0, 0])
send_joint_goal([0.69, 0, 0, 0, 0])
rospy.sleep(1.0)
send_gripper_goal("open")
send_joint_goal([0.69, -1.77, 0, -1.37, 1.57])
return 'done'
@cb_interface(outcomes=['done'])
def _align(_):
robot.head.look_down()
robot.speech.speak("Let's see what I can do with this")
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = rack_id
goal_pose.pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, math.pi))
goal_pose.pose.position.x = 0.45
goal_pose.pose.position.y = 0.09
ControlToPose(
robot, goal_pose,
ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02,
0.05)).execute({})
return 'done'
@cb_interface(outcomes=['done'])
def _grab(_):
robot.speech.speak("Grasping the trolley")
rospy.sleep(4.0)
send_joint_goal([0.61, -1.77, 0, -1.37, 1.57])
send_gripper_goal("close")
base_footprint = [[0.491716563702, 0.284912616014],
[0.504091262817, -0.264433205128],
[0.00334876775742, -0.259195685387],
[-0.17166364193, -0.19022783637],
[-0.239429235458, -0.07719720155],
[-0.237978458405, 0.0547728165984],
[-0.180378556252, 0.164403796196],
[-0.0865250825882, 0.221749901772],
[0.00969874858856, 0.260340631008]]
local_client.update_configuration({"footprint": base_footprint})
global_client.update_configuration({"footprint": base_footprint})
robot.head.cancel_goal()
robot.publish_rack()
return 'done'
@cb_interface(outcomes=['done'])
def _retract(_):
robot.head.look_down()
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = rack_id
goal_pose.pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, -math.pi / 2))
goal_pose.pose.position.x = 0.55
goal_pose.pose.position.y = -0.1
ControlToPose(
robot, goal_pose,
ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02,
0.1)).execute({})
return 'done'
with self:
self.add('PRE_GRAB',
CBState(_pre_grab),
transitions={'done': 'ALIGN_GRAB'})
self.add('ALIGN_GRAB',
CBState(_align),
transitions={'done': 'GRAB'})
self.add('GRAB', CBState(_grab), transitions={'done': 'RETRACT'})
self.add('RETRACT',
CBState(_retract),
transitions={'done': 'done'})
class MyStateMachine(Robot, StateMachine):
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 __del__(self):
self.dclient.update_configuration({"start": False})
self.dclient.update_configuration({"go_home": False})
def callback(self, config, level):
self.start = config['start']
self.go_home = config['go_home']
self.arm_task = config['arm_task']
return config
idle = State('Idle', initial=True)
home = State('Home')
move = State('Move')
arm = State('Arm')
## rmove means Relative move
rmove = State('Rmove')
toIdle = move.to(idle) | home.to(idle) | arm.to(idle) | rmove.to(idle)
toHome = idle.to(home) | move.to(home) | home.to.itself() | arm.to(home)
toMove = idle.to(move) | home.to(move) | move.to.itself() | arm.to(
move) | rmove.to(move)
toArm = move.to(arm) | idle.to(arm)
toRove = idle.to(rmove) | move.to(rmove) | rmove.to.itself()
def on_toIdle(self):
print("To IDLE")
## TODO: STOP the robot arm
# self.call_arm("stop")
print("[IDLE] cancel arm task")
self.cancel_arm_task()
print("[IDLE] clear MiR mission queue")
self.mir.clear_mission_queue()
print("[IDLE] change MiR to 'Pause'")
self.mir.set_status("Pause")
print("[IDLE] Delete mission 'TKU_TMP'")
self.mir.delete_mission("TKU_TMP")
print("[IDLE] reset configures")
self.dclient.update_configuration({"start": False})
self.dclient.update_configuration({"go_home": False})
def on_toHome(self):
self.toMove("TKU_ToHOME")
def on_toMove(self, mission):
self.mir.set_status("Ready")
print("to Move with mission {}".format(mission))
guid = self.mir.get_mission_guid(mission)
if guid is None:
warnings.warn("[WARRING] No this position name!!!!!")
else:
self.mir.add_mission_to_queue(guid.encode('utf-8'))
# elif action.upper() == "RELATIVE MOVE":
# self.mir.relative_move(dx=0.05)
def on_toArm(self, mission=None):
if mission is None:
mission = self.arm_task
print("Call Arm with mission '{}'".format(mission))
self.call_arm(mission)
bridge = CvBridge()
image = None
def callback(image_msg):
try:
cv_image = bridge.imgmsg_to_cv2(image_msg, 'bgr8')
except CvBridgeError as e:
rospy.logerr(e)
res = detect_face(image_msg)
print(res.key_points)
for kp in res.key_points:
cv2.circle(cv_image, (int(kp.x), int(kp.y)), 3, (0, 0, 255), 1)
global image
image = cv_image
if __name__ == '__main__':
rospy.init_node('visualize_openpose')
detect_face = rospy.ServiceProxy('detect_face', DetectKeyPoints)
detect_face.wait_for_service()
reconf = ReconfClient('openpose')
reconf.update_configuration({'key_point_threshold': 0.05})
image_sub = rospy.Subscriber('image', Image, callback)
while not rospy.is_shutdown():
if image is not None:
cv2.imshow('OpenPose visualization', image)
cv2.waitKey(1)
class DelegationManagerTest(unittest.TestCase):
"""
Unit tests for the DelegationManager
"""
def __init__(self, *args, **kwargs):
super(DelegationManagerTest, self).__init__(*args, **kwargs)
rospy.init_node(name="DelegationManagerTest", log_level=rospy.DEBUG)
def setUp(self):
self.mocked_DM_name = "MockedDM"
self.mocked_manager_name = "MockedManager"
self.uut_name = "UUT"
self.uut_mocked_manager_name = "UUTManager"
self.mocked_client = MockedClient()
self.mocked_client_id = self.mocked_client.id
self.mocked_DM = MockedDelegationCommunicator(
name=self.mocked_DM_name, manager_name=self.mocked_manager_name)
self.mocked_cost_eval = MockedCostEvaluator(cost=0, possibility=True)
self.standard_depth = 2
self.standard_members = ["Member1", "Member2"]
self.uut = self.new_uut()
rospy.sleep(0.3)
self.dynrecClient = DynRecClient("UUT/delegation_module_parameters")
self.dynrecClient.update_configuration({"max_tasks": 1})
def tearDown(self):
self.mocked_DM.__del__()
self.uut.__del__()
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 test_setter_getter(self):
"""
Tests the properties
"""
rospy.loginfo("")
uut = self.new_uut()
self.assertEqual(uut.name, self.uut_name)
curr_id = uut.get_new_auction_id()
self.assertEqual(uut.get_new_auction_id(), curr_id + 1)
# TODO this test is running fine if executed manually from pycharm but makes problems from the commandline
@unittest.SkipTest
def test_multiple_tasks(self):
"""
Tests adding tasks
"""
rospy.loginfo("test_multiple_tasks")
id1, id2 = 1, 2
n1, n2 = "name1", "name2"
g1, g2 = "goal1", "goal2"
task1 = Task(auction_id=id1,
auctioneer_name=n1,
goal_name=g1,
depth=self.standard_depth)
task2 = Task(auction_id=id2,
auctioneer_name=n2,
goal_name=g2,
depth=self.standard_depth)
# Max Tasks = -1 (infinite)
self.dynrecClient.update_configuration({"max_tasks": -1})
rospy.sleep(0.3)
uut = self.new_uut()
rospy.sleep(1)
self.assertTrue(uut.check_possible_tasks)
uut.add_task(new_task=task2)
uut.add_task(new_task=task1)
self.assertTrue(uut.check_possible_tasks)
# Max Tasks = 0
self.dynrecClient.update_configuration({"max_tasks": 0})
rospy.sleep(0.3)
uut = self.new_uut()
self.assertFalse(uut.check_possible_tasks)
self.assertRaises(DelegationError, uut.add_task, task1)
self.assertRaises(LookupError, uut.get_task_by_goal_name, g1)
# Max tasks = 1
self.dynrecClient.update_configuration({"max_tasks": 1})
rospy.sleep(0.3)
uut = self.new_uut()
self.assertTrue(uut.check_possible_tasks)
uut.add_task(new_task=task1)
self.assertFalse(uut.check_possible_tasks)
self.assertRaises(DelegationError, uut.add_task, task2)
self.assertEqual(uut.get_task_by_goal_name(goal_name=g1), task1)
self.assertRaises(LookupError, uut.get_task_by_goal_name, g2)
# Max tasks > 1
self.dynrecClient.update_configuration({"max_tasks": 2})
rospy.sleep(0.3)
uut = self.new_uut()
self.assertTrue(uut.check_possible_tasks)
uut.add_task(new_task=task1)
self.assertTrue(uut.check_possible_tasks)
uut.add_task(new_task=task2)
self.assertEqual(uut.get_task_by_goal_name(goal_name=g1), task1)
self.assertEqual(uut.get_task_by_goal_name(goal_name=g2), task2)
self.assertFalse(uut.check_possible_tasks)
def test_cfp_callback(self):
"""
Tests CFP callback
"""
rospy.loginfo("test_cfp_callback")
uut = self.new_uut()
auction_id = 1
goal_rep = "test_goal"
depth = 2
# No Manager present
self.mocked_DM.send_cfp(auction_id=auction_id,
goal_representation=goal_rep,
depth=depth,
delegation_members=self.standard_members)
rospy.sleep(1)
self.assertFalse(self.mocked_DM.got_pro)
# "Manager" present, cost computable
uut.set_cost_function_evaluator(
self.mocked_cost_eval,
client_id=self.mocked_client_id,
agent_name=self.uut_mocked_manager_name)
self.mocked_DM.send_cfp(auction_id=auction_id,
goal_representation=goal_rep,
depth=depth,
delegation_members=self.standard_members)
rospy.sleep(1)
self.assertTrue(self.mocked_DM.got_pro)
res = self.mocked_DM.Pro_last
self.assertEqual(res.name, self.uut_name)
self.assertEqual(res.auction_id, auction_id)
self.assertEqual(res.value, 0) # the evaluator will always compute a 0
self.mocked_DM.reset_messages()
# Deleted Manager
uut.remove_cost_function_evaluator()
self.mocked_DM.send_cfp(auction_id=auction_id,
goal_representation=goal_rep,
depth=depth,
delegation_members=self.standard_members)
rospy.sleep(1)
self.assertFalse(self.mocked_DM.got_pro)
def test_delegate(self):
"""
Tests delegate
"""
rospy.loginfo("test_delegate")
uut = self.new_uut()
goal_name = "test goal"
test_goal = MockedGoalWrapper(name=goal_name)
steps = 2
rospy.sleep(1) # ros msgs directly after startup are lost sometimes
auction_id = uut.delegate(goal_wrapper=test_goal,
auction_steps=steps,
client_id=self.mocked_client_id)
rospy.sleep(1)
self.assertTrue(self.mocked_DM.got_cfp)
cfp = self.mocked_DM.CFP_last
self.assertEqual(cfp.goal_representation, goal_name)
self.assertEqual(cfp.name, self.uut_name)
self.assertEqual(cfp.auction_id, auction_id)
delegation = uut.get_delegation(auction_id=auction_id)
self.assertIsInstance(delegation, Delegation)
self.assertTrue(delegation.state.is_waiting_for_proposals())
self.assertEqual(delegation.auction_id, auction_id)
self.assertEqual(delegation.client_id, self.mocked_client_id)
def test_propose_callback(self):
"""
Tests PROPOSE callback
"""
rospy.loginfo("test_propose_callback")
uut = self.new_uut()
goal_name = "test_goal"
test_goal = MockedGoalWrapper(name=goal_name)
steps = 2
proposed_value = 2
rospy.sleep(1) # ros msgs directly after startup are lost sometimes
auction_id = uut.delegate(goal_wrapper=test_goal,
auction_steps=steps,
client_id=self.mocked_client_id)
rospy.sleep(1)
cfp = self.mocked_DM.CFP_last
self.mocked_DM.send_propose(value=proposed_value,
target_name=cfp.name,
auction_id=cfp.auction_id)
rospy.sleep(1)
delegation = uut.get_delegation(auction_id=auction_id)
proposal = delegation.get_best_proposal()
self.assertEqual(proposal.name, self.mocked_DM_name)
self.assertEqual(proposal.value, proposed_value)
def test_ending_auctions(self):
"""
Tests the Ending of auctions
"""
rospy.loginfo("test_ending_auctions")
uut = self.new_uut()
goal_name = "test_goal"
test_goal = MockedGoalWrapper(name=goal_name)
test_depth = 3
test_task = Task(auction_id=100,
goal_name=goal_name,
depth=test_depth,
auctioneer_name="test")
uut.set_cost_function_evaluator(
client_id=1,
cost_function_evaluator=self.mocked_cost_eval,
agent_name=self.mocked_manager_name)
uut.add_task(test_task)
steps = 3
proposed_value = 2
# No proposals
rospy.sleep(1) # ros msgs directly after startup are lost sometimes
auction_id = uut.delegate(goal_wrapper=test_goal,
auction_steps=steps,
client_id=self.mocked_client_id)
rospy.sleep(1)
self.mocked_DM.reset_messages()
for i in range(steps - 1):
uut.do_step(delegation_ids=[auction_id])
rospy.sleep(1)
self.assertFalse(self.mocked_DM.got_cfp)
uut.do_step(delegation_ids=[auction_id])
rospy.sleep(1)
self.assertTrue(self.mocked_DM.got_cfp)
self.assertEqual(self.mocked_DM.CFP_last.auction_id, auction_id)
uut.terminate(auction_id=auction_id)
# a proposal, but not true anymore, not bidding anymore
test_goal = MockedGoalWrapper(name=goal_name)
auction_id = uut.delegate(goal_wrapper=test_goal,
auction_steps=steps,
client_id=self.mocked_client_id)
rospy.sleep(1)
self.mocked_DM.send_propose(value=proposed_value,
target_name=self.uut_name,
auction_id=auction_id)
self.mocked_DM.reset_messages()
self.mocked_DM.set_precom_response(acceptance=False,
still_bidding=False,
cost=proposed_value)
for i in range(steps):
uut.do_step(delegation_ids=[auction_id])
rospy.sleep(1)
self.assertTrue(self.mocked_DM.got_pre)
self.assertTrue(self.mocked_DM.got_cfp)
uut.terminate(auction_id=auction_id)
# a proposal, but not true anymore, still bidding
test_goal = MockedGoalWrapper(name=goal_name)
auction_id = uut.delegate(goal_wrapper=test_goal,
auction_steps=steps,
client_id=self.mocked_client_id)
rospy.sleep(1)
self.mocked_DM.send_propose(value=proposed_value,
target_name=self.uut_name,
auction_id=auction_id)
self.mocked_DM.reset_messages()
proposed_value += 1
self.mocked_DM.set_precom_response(acceptance=False,
still_bidding=True,
cost=proposed_value)
for i in range(steps):
uut.do_step(delegation_ids=[auction_id])
rospy.sleep(1)
self.assertTrue(self.mocked_DM.got_pre)
self.assertTrue(self.mocked_DM.got_cfp)
uut.terminate(auction_id=auction_id)
# a proposal, but no answer to precommit
test_goal = MockedGoalWrapper(name=goal_name)
auction_id = uut.delegate(goal_wrapper=test_goal,
auction_steps=steps,
client_id=self.mocked_client_id)
rospy.sleep(1)
self.mocked_DM.send_propose(value=proposed_value,
target_name=self.uut_name,
auction_id=auction_id)
self.mocked_DM.reset_messages()
self.mocked_DM.stop_communication()
for i in range(steps):
uut.do_step(delegation_ids=[auction_id])
rospy.sleep(1)
self.assertTrue(self.mocked_DM.got_cfp)
uut.terminate(auction_id=auction_id)
self.mocked_DM.start_communication()
# proposal and accepted precom
test_goal = MockedGoalWrapper(name=goal_name)
auction_id = uut.delegate(goal_wrapper=test_goal,
auction_steps=steps,
client_id=self.mocked_client_id)
rospy.sleep(1)
self.mocked_DM.send_propose(value=proposed_value,
target_name=self.uut_name,
auction_id=auction_id)
self.mocked_DM.reset_messages()
self.mocked_DM.set_precom_response(acceptance=True,
still_bidding=True,
cost=proposed_value)
for i in range(steps):
uut.do_step(delegation_ids=[auction_id])
rospy.sleep(1)
self.assertTrue(self.mocked_DM.got_pre)
self.assertEqual(self.mocked_DM.Pre_last.depth, test_depth + 1)
self.assertTrue(test_goal.goal_is_created())
self.assertEqual(test_goal.get_manager(), self.mocked_manager_name)
uut.terminate(auction_id=auction_id)
# own cost wins
test_goal = MockedGoalWrapper(name=goal_name)
own_cost = 2
auction_id = uut.delegate(goal_wrapper=test_goal,
auction_steps=steps,
client_id=self.mocked_client_id,
own_cost=own_cost)
for i in range(steps):
uut.do_step(delegation_ids=[auction_id])
rospy.sleep(1)
self.assertFalse(test_goal.goal_is_created())
self.assertTrue(self.mocked_client.started_working)
# TODO this test is running fine if executed manually from pycharm but makes problems from the commandline
@unittest.SkipTest
def test_terminate(self):
"""
Tests terminate
"""
rospy.loginfo("test_terminate")
uut = self.new_uut()
goal_name = "test_goal"
test_goal = MockedGoalWrapper(name=goal_name)
steps = 3
proposed_value = 2
# proposal and accepted precom
auction_id = uut.delegate(goal_wrapper=test_goal,
auction_steps=steps,
client_id=self.mocked_client_id)
rospy.sleep(1)
self.mocked_DM.send_propose(value=proposed_value,
target_name=self.uut_name,
auction_id=auction_id)
self.mocked_DM.reset_messages()
self.mocked_DM.set_precom_response(acceptance=True,
still_bidding=True,
cost=proposed_value)
for i in range(steps):
uut.do_step(delegation_ids=[auction_id])
rospy.sleep(1)
self.assertTrue(self.mocked_DM.got_pre)
self.assertTrue(test_goal.goal_is_created())
self.assertEqual(test_goal.get_manager(), self.mocked_manager_name)
uut.terminate(auction_id=auction_id)
delegation = uut.get_delegation(auction_id=auction_id)
self.assertFalse(test_goal.goal_is_created())
self.assertTrue(delegation.state.is_finished())
# TODO this test is running fine if executed manually from pycharm but makes problems from the commandline
@unittest.SkipTest
def test_precom_callback(self):
"""
Tests PRECOM callback
"""
rospy.loginfo("test_precom_callback")
auction_id = 1
goal_name = "test_goal"
old_proposal = 3
# cost not computable
uut = self.new_uut()
response = self.mocked_DM.send_precom(
target_name=self.uut_name,
auction_id=auction_id,
proposal_value=old_proposal,
goal_name=goal_name,
goal_representation=goal_name,
depth=self.standard_depth,
delegation_members=self.standard_members)
self.assertFalse(response.acceptance)
self.assertFalse(response.still_biding)
# max tasks = 0
self.dynrecClient.update_configuration({"max_tasks": 0})
rospy.sleep(0.3)
uut = self.new_uut()
uut.set_cost_function_evaluator(
cost_function_evaluator=self.mocked_cost_eval,
agent_name=self.uut_mocked_manager_name,
client_id=self.mocked_client_id)
response = self.mocked_DM.send_precom(
target_name=self.uut_name,
auction_id=auction_id,
proposal_value=old_proposal,
goal_name=goal_name,
goal_representation=goal_name,
depth=self.standard_depth,
delegation_members=self.standard_members)
self.assertFalse(response.acceptance)
self.assertFalse(response.still_biding)
# not possible anymore
self.dynrecClient.update_configuration({"max_tasks": 1})
rospy.sleep(0.3)
uut = self.new_uut()
uut.set_cost_function_evaluator(
cost_function_evaluator=MockedCostEvaluator(cost=0,
possibility=False),
agent_name=self.uut_mocked_manager_name,
client_id=self.mocked_client_id)
response = self.mocked_DM.send_precom(
target_name=self.uut_name,
auction_id=auction_id,
proposal_value=old_proposal,
goal_name=goal_name,
goal_representation=goal_name,
depth=self.standard_depth,
delegation_members=self.standard_members)
self.assertFalse(response.acceptance)
self.assertFalse(response.still_biding)
# cost is worse now
new_cost = old_proposal + 1
self.dynrecClient.update_configuration({"max_tasks": 1})
rospy.sleep(0.3)
uut = self.new_uut()
uut.set_cost_function_evaluator(
cost_function_evaluator=MockedCostEvaluator(cost=new_cost,
possibility=True),
agent_name=self.uut_mocked_manager_name,
client_id=self.mocked_client_id)
response = self.mocked_DM.send_precom(
target_name=self.uut_name,
auction_id=auction_id,
proposal_value=old_proposal,
goal_name=goal_name,
goal_representation=goal_name,
depth=self.standard_depth,
delegation_members=self.standard_members)
self.assertFalse(response.acceptance)
self.assertTrue(response.still_biding)
self.assertEqual(response.new_proposal, new_cost)
# cost is exactly the same
self.dynrecClient.update_configuration({"max_tasks": -1})
rospy.sleep(0.3)
new_cost = old_proposal
uut = self.new_uut()
uut.set_cost_function_evaluator(
cost_function_evaluator=MockedCostEvaluator(cost=new_cost,
possibility=True),
agent_name=self.uut_mocked_manager_name,
client_id=self.mocked_client_id)
response = self.mocked_DM.send_precom(
target_name=self.uut_name,
auction_id=auction_id,
proposal_value=old_proposal,
goal_name=goal_name,
goal_representation=goal_name,
depth=self.standard_depth,
delegation_members=self.standard_members)
self.assertTrue(response.acceptance)
self.assertEqual(response.manager_name, self.uut_mocked_manager_name)
def test_cost_function_adding_removing(self):
"""
Tests CostEvaluator adding/removing
"""
rospy.loginfo("test_cost_function_adding_removing")
uut = self.new_uut()
uut.set_cost_function_evaluator(
cost_function_evaluator=self.mocked_cost_eval,
agent_name=self.uut_mocked_manager_name,
client_id=self.mocked_client_id)
self.assertTrue(uut.cost_computable)
uut.remove_cost_function_evaluator()
self.assertFalse(uut.cost_computable)
def test_fail_task(self):
"""
Tests FAIL callback
"""
rospy.loginfo("test_fail_task")
uut = self.new_uut()
auction_id = 1
goal_name = "test goal"
task = Task(auction_id=auction_id,
auctioneer_name=self.mocked_DM_name,
goal_name=goal_name,
depth=self.standard_depth)
uut.add_task(new_task=task)
uut.fail_task(goal_name=goal_name)
rospy.sleep(1)
self.assertTrue(self.mocked_DM.got_fail)
request = self.mocked_DM.Fail_last
self.assertEqual(self.uut_name, request.name)
self.assertEqual(request.auction_id, auction_id)
def test_end_task(self):
"""
Tests end task
"""
rospy.loginfo("test_end_task")
uut = self.new_uut()
auction_id = 1
goal_name = "test goal"
task = Task(auction_id=auction_id,
auctioneer_name=self.mocked_DM_name,
goal_name=goal_name,
depth=self.standard_depth)
uut.add_task(new_task=task)
uut.end_task(goal_name=goal_name)
self.assertRaises(LookupError, uut.get_task_by_goal_name, goal_name)
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)
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