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)
# 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)
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:
class FlockingNode:
"""Implements vision-based flocking algorithm."""
def __init__(self):
self.node_name = 'flocking_node'
rospy.init_node(self.node_name, argv=sys.argv)
param_n = '~n'
self.n = rospy.get_param(param_n, default=None)
if self.n is None:
rospy.logerr('Cannot read parameter: {}'.format(param_n))
exit()
# Try connecting to global dynamic reconfigure server, otherwise create local one
self.config = argparse.Namespace()
try:
self.client = Client('/gcs/flocking_node_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(FlockingNodeConfig,
callback=self.config_callback)
self.client = Client(self.node_name)
self.config = argparse.Namespace(**self.client.get_configuration())
self.my_id = int(rospy.get_namespace().split('/')[1].split('_')[-1])
rospy.loginfo('My ID: {}'.format(self.my_id))
self.ids = set(range(1, self.n + 1)) - set([self.my_id
]) # Set difference!
rospy.loginfo('Other IDs: {}'.format(list(self.ids)))
self.last_command = np.zeros(3)
self.detections = []
# Transform
self.buffer = tf2_ros.Buffer(rospy.Duration(1.0))
self.listener = tf2_ros.TransformListener(self.buffer)
# Publisher
self.pub_setpoint = rospy.Publisher('mavros/setpoint_raw/local',
PositionTarget,
queue_size=1)
# Subscribers
self.pose = None
self.altitude = None
topic_pose = 'pose'
self.sub_pose = rospy.Subscriber(topic_pose,
PoseStamped,
callback=self.pose_callback,
queue_size=1)
topic_detections = 'detections'
self.sub_detections = rospy.Subscriber(
topic_detections,
Detection3DArray,
callback=self.detections_callback)
self.waypoint = None
topic_migration = 'migration'
self.sub_migration = rospy.Subscriber(topic_migration,
PoseStamped,
callback=self.migration_callback)
self.poses = {}
self.subs_poses = {}
for identity in self.ids:
namespace = '/drone_{}/'.format(identity)
topic = re.sub('^/drone_[1-9]/', namespace, self.sub_pose.name)
sub_pose = rospy.Subscriber(topic,
PoseStamped,
callback=self.poses_callback,
callback_args=identity)
self.subs_poses[identity] = sub_pose
def migration_callback(self, pose_msg):
rospy.loginfo_once('Got migration setpoint callback')
target_frame = f'drone_{self.my_id}/base_link'
source_frame = 'world'
try:
transform = self.buffer.lookup_transform(target_frame=target_frame,
source_frame=source_frame,
time=rospy.Time(0))
except Exception as e:
print(e)
return
point = PointStamped()
point.header = pose_msg.header
point.point.x = pose_msg.pose.position.x
point.point.y = pose_msg.pose.position.y
point.point.z = pose_msg.pose.position.z
point_tf = tf2_geometry_msgs.do_transform_point(point, transform)
self.waypoint = point_tf
def pose_callback(self, pose_msg):
"""Callback for my own pose"""
rospy.loginfo_once('Got own pose callback')
self.pose = pose_msg
self.altitude = pose_msg.pose.position.z
def poses_callback(self, pose_msg, identity):
"""Callback for poses of other agents"""
rospy.loginfo_once('Got other poses callback')
if self.pose is None:
return
target_frame = f'drone_{self.my_id}/base_link'
source_frame = 'world'
try:
transform = self.buffer.lookup_transform(target_frame=target_frame,
source_frame=source_frame,
time=rospy.Time(0))
except Exception as e:
print(e)
return
point = PointStamped()
point.header = pose_msg.header
point.point.x = pose_msg.pose.position.x
point.point.y = pose_msg.pose.position.y
point.point.z = pose_msg.pose.position.z
point_tf = tf2_geometry_msgs.do_transform_point(point, transform)
p = point_tf.point
self.poses[identity] = np.array([p.x, p.y, p.z])
def detections_callback(self, detection_array_msg):
rospy.loginfo_once('Got visual detections callback')
self.detections = []
for detection in detection_array_msg.detections:
pos = detection.bbox.center.position
self.detections.append(np.array([pos.x, pos.y, pos.z]))
def config_callback(self, config, level=None):
old_config = vars(self.config)
for name, new_value in config.items():
if name == 'groups' or name not in old_config:
continue
old_value = old_config[name]
if old_value != new_value:
rospy.loginfo('Update `{}`: {} -> {}'.format(
name, old_value, new_value))
return self.handle_config(config)
def handle_config(self, config):
self.config = argparse.Namespace(**config)
return config
def flock(self):
# Command from reynolds
command_reynolds = self.get_command_reynolds()
# Low-pass filter
alpha = self.config.smoothing_factor
command = alpha * command_reynolds + (1 - alpha) * self.last_command
self.last_command = command
# For experiments
command *= self.config.command_gain
# Altitude controller
if self.config.use_altitude and self.altitude is not None:
altitude_error = self.config.altitude_setpoint - self.altitude
altitude_command = altitude_error * self.config.altitude_gain
command[2] = altitude_command
# Add migration term to command
command += self.get_command_migration()
# Limit maximum speed to max_speed
if np.linalg.norm(command) > self.config.max_speed:
command /= np.linalg.norm(command) # Make unit vector
command *= self.config.max_speed # Scale by max speed
# Publish velocity setpoint in ENU body frame
frame_id = f'/drone_{self.my_id}/base_link'
setpoint_msg = setpoint_util.velocity_target(command,
frame_id=frame_id)
setpoint_msg.coordinate_frame = setpoint_msg.FRAME_BODY_NED
setpoint_msg.yaw = np.deg2rad(
0.) # Setting to zero not necessary for stability!
self.pub_setpoint.publish(setpoint_msg)
def get_command_migration(self):
# Return early with zero command if possible
migration_gain = self.config.migration_gain
if migration_gain <= 0.0 or self.pose is None or self.waypoint is None:
return np.zeros(3)
# Return zero command if the last waypoint message is older than one second
if rospy.Time.now() - self.waypoint.header.stamp > rospy.Duration(1):
rospy.logwarn(
'No longer receiving new waypoints. Stopping migration.')
self.waypoint = None
return np.zeros(3)
p = self.waypoint.point
pos_waypoint = np.array([p.x, p.y, 0]) # Set z component to zero!
migration_direction = pos_waypoint / np.linalg.norm(pos_waypoint)
command_migration = migration_direction * migration_gain
return command_migration
def get_command_reynolds(self):
# Decide if flocking based on true poses or visual detections
if self.config.use_vision:
positions_rel = self.detections
else:
positions_rel = list(self.poses.values())
# Only flock when we have relative positions
if len(positions_rel) == 0:
return np.zeros(3)
distances = [np.linalg.norm(r) for r in positions_rel]
agents_outside = [
d for d in distances if d > self.config.perception_radius
]
if len(agents_outside):
msg = 'Agents outside of perception radius: {}'.format(
len(agents_outside))
rospy.logwarn_throttle(1.0, msg)
command_reynolds = reynolds(
positions_rel,
separation_gain=self.config.separation_gain,
cohesion_gain=self.config.cohesion_gain,
alignment_gain=self.config.alignment_gain,
perception_radius=self.config.perception_radius,
max_agents=self.config.max_agents)
return command_reynolds
class MultiTargetTrackingNode:
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 velocity_callback(self, twist_msg):
rospy.loginfo_once('Got velocity callback')
v = twist_msg.twist.linear
self.velocity = np.array([v.x, v.y, v.z])
def config_callback(self, config, level=None):
for name, new_value in config.items():
if name == 'groups' or name not in self.config:
continue
old_value = self.config[name]
if old_value != new_value:
rospy.loginfo('Update `{}`: {} -> {}'.format(
name, old_value, new_value))
return self.update_config(config)
def update_config(self, config):
self.config = config
return config
def filter_dbscan(self, detections_msg):
# Get frame_id for drone body frame: 'drone_N/camera_array' -> 'drone_N'
frame_id = detections_msg.header.frame_id.split('/')[0] + '/base_link'
# Return early if we only have zero or one detection
if len(detections_msg.detections) <= 1:
detections_msg.header.frame_id = frame_id
return detections_msg
positions = []
for detection in detections_msg.detections:
p = detection.bbox.center.position
position = np.array([p.x, p.y]) if self.dimensions == 2 \
else np.array([p.x, p.y, p.z])
positions.append(position)
positions = np.array(positions)
# Greedily find all positions with distance less than threshold
self.dbscan.fit(positions)
# Merge close positions as the average of all cluster positions
labels = np.array(self.dbscan.labels_)
indices = set(labels)
filtered = np.array(
[positions[labels == i].mean(axis=0) for i in indices])
filtered_detections = Detection3DArray()
filtered_detections.header.frame_id = frame_id
filtered_detections.header.stamp = detections_msg.header.stamp
for filtered_detection in filtered:
detection = Detection3D()
detection.header = detections_msg.header
detection.bbox.center.position.x = filtered_detection[0]
detection.bbox.center.position.y = filtered_detection[1]
if self.dimensions == 3:
detection.bbox.center.position.z = filtered_detection[2]
filtered_detections.detections.append(detection)
return filtered_detections
def filter_gmphd(self, detections_msg):
# Check when the last detection message arrived
if self.last_detections_msg is None:
self.last_detections_msg = detections_msg
rospy.loginfo('GMPHD filter: last detection message initialized')
return detections_msg
# Compute time difference (dt) from last message timestamp
last_detection_time = self.last_detections_msg.header.stamp.to_sec()
this_detection_time = detections_msg.header.stamp.to_sec()
dt = this_detection_time - last_detection_time
self.last_detections_msg = detections_msg
# Set dt for those GM-PHD filters that have it (e.g. EK-PHD)
if hasattr(self.gmphd, 'dt'):
self.gmphd.dt = dt
# Update process noise covariance matrix (which depends on dt)
self.gmphd.Q = self._compute_process_noise_covariance(dt)
# Generate birth components for PHD filter
birth_components = []
for detection in detections_msg.detections:
birth_component = deepcopy(self.gmphd.birth_component)
p = detection.bbox.center.position
birth_component.weight = self.config['birth_weight']
birth_component.mean[0] = p.x
birth_component.mean[1] = p.y
if self.dimensions == 3:
birth_component.mean[2] = p.z
birth_components.append(birth_component)
self.gmphd.birth_components = birth_components
# Convert observations from cartesian into spherical coordinates
observations = []
for detection in detections_msg.detections:
p = detection.bbox.center.position
observation = cartesian2polar([p.x, p.y]) if self.dimensions == 2 \
else cartesian2spherical([p.x, p.y, p.z])
observations.append(observation)
observations = np.array(observations)
# Prepare control inputs (estimated velocity)
control_inputs = np.zeros(self.gmphd.dim_x)
if self.velocity is not None:
control_inputs[:self.dimensions] = self.velocity[:self.dimensions]
control_inputs = control_inputs[..., np.newaxis]
# Update filter
self.gmphd.filter(observations, control_inputs)
# Prune
self.gmphd.prune(trunc_thresh=self.config['trunc_thresh'],
merge_thresh=self.config['merge_thresh'],
max_components=self.config['max_components'])
# Get frame_id for drone body frame: 'drone_N/camera_array' -> 'drone_N'
frame_id = detections_msg.header.frame_id.split('/')[0] + '/base_link'
# Publishes the biggest component as PoseWithCovariance (for visualization only!)
if len(self.gmphd.components) > 0:
comp = self.gmphd.components[0]
pose_cov_msg = PoseWithCovarianceStamped()
pose_cov_msg.header.frame_id = frame_id
pose_cov_msg.header.stamp = rospy.Time.now()
# Use mean as position
pose_cov_msg.pose.pose.position.x = comp.mean[0]
pose_cov_msg.pose.pose.position.y = comp.mean[1]
pose_cov_msg.pose.pose.position.z = comp.mean[2]
# Prepare covariance matrix
covariance = np.zeros((6, 6)) # 6x6 row-major matrix
dimz = self.gmphd.dim_z
covariance[:dimz, :
dimz] = comp.cov[:dimz, :
dimz] # Need only position covariance
pose_cov_msg.pose.covariance = list(covariance.flatten())
self.pose_cov_pub.publish(pose_cov_msg)
filtered_detections = Detection3DArray()
filtered_detections.header.frame_id = frame_id
filtered_detections.header.stamp = rospy.Time.now()
for comp in self.gmphd.components:
# Only report components above weight threshold as detections
if comp.weight < self.config['weight_thresh']:
continue
detection = Detection3D()
detection.header = detections_msg.header
detection.bbox.center.position.x = comp.mean[0]
detection.bbox.center.position.y = comp.mean[1]
detection.bbox.size.x = 2 * np.sqrt(comp.cov[0, 0])
detection.bbox.size.y = 2 * np.sqrt(comp.cov[1, 1])
detection.bbox.size.z = 0.1 # rviz complains about scale 0 otherwise
if self.dimensions == 3:
detection.bbox.center.position.z = comp.mean[2]
detection.bbox.size.z = 2 * np.sqrt(comp.cov[2, 2])
filtered_detections.detections.append(detection)
return filtered_detections
def detections_callback(self, raw_msg):
use_dbscan, use_gmphd = self.config['use_dbscan'], self.config[
'use_gmphd']
filtered_msg = Detection3DArray()
filtered_msg.header = raw_msg.header
# Filter detections of same object from multiple cameras using DBSCAN
# DBSCAN guarantees that we only have one observation per true object
filtered_msg = self.filter_dbscan(raw_msg) if use_dbscan else raw_msg
# Filter detections with GM-PHD filter
# GM-PHD requires at most one observation per true object
filtered_msg = self.filter_gmphd(
filtered_msg) if use_gmphd else filtered_msg
# Log information
# num_raw = len(raw_msg.detections)
num_filtered = len(filtered_msg.detections)
# rospy.loginfo('Detections raw/filtered: {}/{}'.format(num_raw, num_filtered))
# Publish everything
self.detections_pub.publish(filtered_msg)
self.num_target_pub.publish(Int32(num_filtered))
def _compute_process_noise_covariance(self, dt):
"""Compute piecewise white noise covariance matrix based on dt
Source: https://en.wikipedia.org/wiki/Kalman_filter
"""
I2 = np.eye(self.dimensions)
Q = np.block([[1. / 4. * dt**4 * I2, 1. / 2. * dt**3 * I2],
[1. / 2. * dt**3 * I2, dt**2 * I2]])
Q *= self.config['process_noise']**2
return Q
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 ObjectDetectionNode:
def __init__(self):
self.node_name = 'object_detection_node'
rospy.init_node(self.node_name, argv=sys.argv)
# Parameters
self.type = rospy.get_param('~type', default='yolo')
self.checkpoint = rospy.get_param('~checkpoint', default=None)
self.config = rospy.get_param('~config', default=None)
self.grayscale = rospy.get_param('~grayscale', default=False)
self.verbose = rospy.get_param('~verbose', default=False)
self.namespace = rospy.get_namespace().split('/')[1]
# Choose detector type
rospy.loginfo('Loading detector: {}'.format(self.type))
if self.type == 'apriltag':
from vswarm.object_detection.apriltag_detector import ApriltagDetector
self.detector = ApriltagDetector()
elif self.type == 'blob':
from vswarm.object_detection.blob_detector import BlobDetector
self.detector = BlobDetector()
elif self.type == 'checkerboard':
from vswarm.object_detection.checkerboard_detector import CheckerboardDetector
self.detector = CheckerboardDetector(checkerboard_shape=(8, 6))
elif self.type == 'yolo':
from vswarm.object_detection.yolo_detector import YoloDetector
self.detector = YoloDetector(checkpoint_path=self.checkpoint,
config_path=self.config,
grayscale=self.grayscale,
verbose=self.verbose)
rospy.loginfo('Loaded config: {}'.format(
os.path.basename(self.config)))
else:
rospy.logerr('Unrecognized detector type: {}'.format(self.type))
exit(1)
rospy.loginfo('Detector loaded: {}'.format(self.type))
# Try connecting to global dynamic reconfigure server, otherwise create local one
self.config = {}
try:
self.client = Client('/gcs/object_detection_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(ObjectDetectionNodeConfig,
callback=self.config_callback)
self.client = Client(self.node_name)
self.config = self.client.get_configuration()
self.bridge = CvBridge()
# Visual tracking
self.track_length = 0
self.trackers = []
self.initial_detections_list = []
# Publishers
detections_pub_topic = self.node_name + '/detections'
self.detections_pub = rospy.Publisher(detections_pub_topic,
Detection2DArray,
queue_size=1)
num_detections_pub_topic = self.node_name + '/num_detections'
self.num_detections_pub = rospy.Publisher(num_detections_pub_topic,
Int32,
queue_size=1)
self.input_images = rospy.get_param('~input_images')
image_topic = self.node_name + '/detections/image_raw'
self.image_publisher = rospy.Publisher(image_topic,
Image,
queue_size=1)
self.image_messages = [None for _ in self.input_images]
self.image_updated = [False for _ in self.input_images]
self.image_subscribers = []
for i, topic in enumerate(self.input_images):
# Subscriber
subscriber = rospy.Subscriber(topic,
Image,
callback=self.image_callback,
callback_args=i,
queue_size=1,
buff_size=2**24)
self.image_subscribers.append(subscriber)
def config_callback(self, config, level=None):
for name, new_value in config.items():
if name == 'groups' or name not in self.config:
continue
old_value = self.config[name]
if old_value != new_value:
rospy.loginfo('Update `{}`: {} -> {}'.format(
name, old_value, new_value))
return self.update_config(config)
def update_config(self, config):
self.config = config
# Update underlying detector
if hasattr(self.detector, 'confidence_threshold'):
self.detector.confidence_threshold = self.config[
'confidence_threshold']
if hasattr(self.detector, 'iou_threshold'):
self.detector.iou_threshold = self.config['iou_threshold']
return config
def image_callback(self, image_msg, i):
rospy.loginfo_once('Image callback')
self.image_messages[i] = image_msg
self.image_updated[i] = True
if all(self.image_updated):
self.image_updated = [False for _ in self.input_images]
self.callback(*self.image_messages[:])
def callback(self, *image_msgs):
# Extract width and height from image message
width, height = image_msgs[0].width, image_msgs[0].height
# IMPORTANT: if frame_id of image_msg is not fully qualified, we'll do it here
# This is needed to associate the image with the proper TF and calibration
# The fully qualified frame_id is: drone_<N>/camera_<direction>_optical
for image_msg, topic in zip(image_msgs, self.input_images):
# In reality, the frame_id does not start with 'drone_'
if not image_msg.header.frame_id.startswith('drone_'):
camera_name = topic.split('/')[-2]
image_msg.header.frame_id = '{}/{}_optical'.format(
self.namespace, camera_name)
# In simulation, the frame_id ends with '_link' which we don't want
if image_msg.header.frame_id.endswith('_link'):
frame_id = image_msg.header.frame_id.replace(
'_link', '_optical')
image_msg.header.frame_id = frame_id
# Convert ROS image messages to grayscale numpy images
images = []
for image_msg in image_msgs:
image_raw = self.bridge.imgmsg_to_cv2(image_msg)
if image_raw.ndim == 3:
image_raw = cv.cvtColor(image_raw, cv.COLOR_BGR2GRAY)
images.append(image_raw)
# Optional: introduce processing delay
if self.config['delay'] > 0.0:
rospy.sleep(self.config['delay'])
if self.track_length > self.config['max_track_length']:
self.track_length = 0
# Detect or track
detection_array_msg_list = []
if self.config['use_visual_tracking'] and self.track_length > 0:
rospy.logdebug('Tracking: {}/{}'.format(
self.track_length, self.config['max_track_length']))
for i, (tracker, image, dets) in enumerate(
zip(self.trackers, images, self.initial_detections_list)):
if tracker is not None:
tracked = tracker.track(image)
if len(tracked.detections) < len(dets.detections):
rospy.logdebug('Lost track!')
self.trackers[i] = None
else:
tracked = Detection2DArray()
detection_array_msg_list.append(tracked)
self.track_length += 1
else:
# Input: list of images, output: list of detection arrays
rospy.logdebug('Detection')
detection_array_msg_list = self.detector.detect_multi(images)
# Create trackers only for images for which we have detections
if self.config['use_visual_tracking'] and self.track_length == 0:
self.trackers = []
self.initial_detections_list = detection_array_msg_list
rospy.logdebug('Tracking: feeding detection to tracker')
for image, detection_array_msg in zip(
images, self.initial_detections_list):
if len(detection_array_msg.detections) > 0:
tracker = OpenCVTracker(image,
detection_array_msg.detections)
else:
tracker = None
self.trackers.append(tracker)
self.track_length += 1
# Filter out edge detections and (optionally) add false negatives
out_detection_array_msg_list = []
for detection_array_msg, image_msg in zip(detection_array_msg_list,
image_msgs):
out_detection_array = Detection2DArray()
for detection_msg in detection_array_msg.detections:
# Reject detections based distance to the image edges
if util.is_edge_detection(detection_msg,
width,
height,
width_thresh=0.01,
height_thresh=0.01):
continue
# Drop detections with false negative probability
if bool(
np.random.binomial(
1, p=self.config['false_negative_prob'])):
continue
# Add out detection with image header to detections
# We add the image header to match image to detection later on
out_detection = detection_msg
out_detection.header = image_msg.header
out_detection_array.detections.append(detection_msg)
out_detection_array_msg_list.append(out_detection_array)
# Aggregate results from multiple detection arrays into one (with proper frame_ids)
out_detection_array_msg = Detection2DArray()
out_detection_array_msg.header.stamp = rospy.Time.now()
# The following frame_id does not actually exist (check individual detections!)
frame_id = image_msgs[0].header.frame_id.split(
'/')[0] + '/camera_array'
out_detection_array_msg.header.frame_id = frame_id
for detection_array_msg in out_detection_array_msg_list:
for detection_msg in detection_array_msg.detections:
out_detection_array_msg.detections.append(detection_msg)
# Publish detections
self.detections_pub.publish(out_detection_array_msg)
self.num_detections_pub.publish(
Int32(len(out_detection_array_msg.detections)))
# Publish image annotated with detections (optionally)
if self.config['publish_image']:
images_annotated = []
for image, detection_array_msg in zip(
images, out_detection_array_msg_list):
image_annotated = cv.cvtColor(image, cv.COLOR_GRAY2BGR)
for detection_msg in detection_array_msg.detections:
image_annotated = self._draw_detection(
image_annotated, detection_msg)
images_annotated.append(image_annotated)
# Concatenate images along width axis
# image_concat = self._make_collage(images_annotated)
image_concat = self._make_image_strip(images_annotated)
# Publish image with detection overlay
image_detection_msg = self.bridge.cv2_to_imgmsg(image_concat,
encoding='bgr8')
image_detection_msg.header.stamp = rospy.Time.now()
self.image_publisher.publish(image_detection_msg)
# Keep previous detection list for tracking
self.previous_detection_list = out_detection_array_msg
def _draw_detection(self, image, detection_msg):
"""Draw detection bounding box and text"""
pt1, pt2 = util.detection_2d_to_points(detection_msg)
color = (0, 0, 255) if self.track_length == 0 else (0, 255, 0)
image = cv.rectangle(image, pt1=pt1, pt2=pt2, color=color, thickness=2)
if len(detection_msg.results) != 0:
object_hypothesis = detection_msg.results[0]
score = object_hypothesis.score * 100
# name = 'Drone' # object_hypothesis.id # TODO: human-readable name
text = '{score}%'.format(score=int(round(score)))
x, y = pt1
org = (x, y - 5)
image = cv.putText(image,
text=text,
org=org,
fontFace=cv.FONT_HERSHEY_COMPLEX,
fontScale=0.4,
color=color,
thickness=1,
lineType=cv.LINE_AA)
return image
def _make_image_strip(self, image_list):
return np.concatenate(image_list, axis=1)
def _make_collage(self, image_list):
height, width, channels = image_list[0].shape
dtype = image_list[0].dtype
size = int(np.ceil(np.sqrt(len(image_list)))) # Grid: size x size
collage = np.zeros((size * height, size * width, channels),
dtype=dtype)
index = 0
for col in range(size):
for row in range(size):
if index > len(image_list):
break
h, w = row * height, col * width
collage[h:h + height, w:w + width, :] = image_list[index]
# Add text
text = self.input_images[index]
org = (w + int(0.03 * width), h + height - int(0.03 * height))
collage = cv.putText(collage,
text=text,
org=org,
fontFace=cv.FONT_HERSHEY_COMPLEX,
fontScale=0.6,
color=(255, 255, 255),
thickness=1,
lineType=cv.LINE_AA)
index += 1
return collage
rospy.loginfo("Configuration callback")
return
if __name__ == "__main__":
rospy.init_node("webviz_test_client")
client = Client("webviz_test_server", timeout=30, config_callback=callback)
pub = rospy.Publisher("sin", std_msgs.msg.Float64, queue_size=1)
r = rospy.Rate(10)
t0 = rospy.Time.now()
while not rospy.is_shutdown():
config = client.get_configuration()
A = config.amplitude
w = config.freq
p = config.phase
name = config.name
v_flag = config.flag
v_int = config.int
v_size = config.size
t1 = rospy.Time.now()
t = (t1 - t0).to_sec()
val = std_msgs.msg.Float64()
val.data = A * sin(2 * M_PI * w * t + p)
# 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)
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:
class RelativeLocalizationNode:
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 config_callback(self, config, level=None):
for name, new_value in config.items():
if name == 'groups' or name not in self.config:
continue
old_value = self.config[name]
if old_value != new_value:
rospy.loginfo('Update `{}`: {} -> {}'.format(
name, old_value, new_value))
return self.update_config(config)
def update_config(self, config):
self.config = config
return config
def detections_callback(self, detections_msg):
rospy.loginfo_once('Got detections')
out_detections_msg = Detection3DArray()
out_detections_msg.header.stamp = rospy.Time.now()
out_detections_msg.header.frame_id = detections_msg.header.frame_id
for detection in detections_msg.detections:
# Get localizer based on detection frame_id
frame_id = detection.header.frame_id # drone_X/camera_X_optical
camera_ns = frame_id.split('/')[-1].replace('_optical',
'') # camera_X
localizer = self.localizers[camera_ns]
# Calculate unit-norm bearing from bounding box and physical object size
bbox_center = (detection.bbox.center.x, detection.bbox.center.y)
bbox_size = (detection.bbox.size_x, detection.bbox.size_y)
object_width = self.config['object_width']
object_height = self.config['object_height']
object_depth = self.config['object_depth']
object_size = (object_width, object_height, object_depth)
bearing = localizer.detection_to_bearing(bbox_center, bbox_size,
object_size)
# Transform bearing vector from camera/optical frame to body/base_link frame
source_frame = detection.header.frame_id # drone_X/camera_X_optical
target_frame = source_frame.split('/')[0] + '/base_link'
try:
transform = self.buffer.lookup_transform(
target_frame=target_frame,
source_frame=source_frame,
time=rospy.Time(0))
except Exception as e:
print(e)
continue
point = PointStamped()
point.header.frame_id = source_frame
point.header.stamp = rospy.Time.now()
point.point.x = bearing[0]
point.point.y = bearing[1]
point.point.z = bearing[2]
point_tf = tf2_geometry_msgs.do_transform_point(point, transform)
out_detection = Detection3D()
out_detection.header = point_tf.header
out_detection.bbox.center.position.x = point_tf.point.x
out_detection.bbox.center.position.y = point_tf.point.y
out_detection.bbox.center.position.z = point_tf.point.z
out_detection.bbox.size.x = object_depth
out_detection.bbox.size.y = object_width
out_detection.bbox.size.z = object_height
out_detections_msg.detections.append(out_detection)
self.detections_pub.publish(out_detections_msg)
class MigrationNode:
"""Implements swarm migration."""
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 config_callback(self, config, level):
self.config = argparse.Namespace(**config)
return config
def pose_callback(self, pose_msg, i):
rospy.loginfo_once('Got poses')
p = pose_msg.pose.position
self.positions[i] = np.array([p.x, p.y, p.z])
def publish_migration_setpoint(self):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = 'world'
wp = self.waypoints[self.current_waypoint]
pose.pose.position.x = wp[0]
pose.pose.position.y = wp[1]
pose.pose.position.z = wp[2]
self.setpoint_pub.publish(pose)
def publish_migration_markers(self):
markers = MarkerArray()
for i, wp in enumerate(self.waypoints):
marker = Marker()
marker.header.frame_id = 'world'
marker.header.stamp = rospy.Time.now()
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.id = i
marker.pose.position.x = wp[0]
marker.pose.position.y = wp[1]
marker.pose.position.z = 0.0
marker.pose.orientation.w = 1.0 # Initialize quaternion!
marker.color.a = 0.3
marker.color.r = 0.7
marker.color.b = 0.7
marker.color.g = 0.7
# visualize current waypoint
if i == self.current_waypoint:
marker.color.g = 1.0
marker.scale.x = self.config.acceptance_radius
marker.scale.y = self.config.acceptance_radius
marker.scale.z = self.config.acceptance_radius
markers.markers.append(marker)
self.waypoint_pub.publish(markers)
def run(self):
rate = rospy.Rate(10.)
while not rospy.is_shutdown():
rate.sleep()
if any(x is None for x in self.positions):
rospy.logwarn_throttle(3,
'Waiting for `{}`.'.format(self.poses))
continue
position_mean = np.array(self.positions).mean(axis=0)
# Check if waypoint is reached
position_waypoint = self.waypoints[self.current_waypoint]
position_error = position_waypoint - position_mean
# TODO: currently ignores z component of waypoints
distance = np.linalg.norm(position_error[:2])
if distance < self.config.acceptance_radius:
rospy.loginfo('Waypoint {}/{} reached.'.format(
self.current_waypoint + 1, len(self.waypoints)))
# Cycle through waypoints (in reversed order if desired)
increment = -1 if self.config.reversed else 1
self.current_waypoint += increment
self.current_waypoint %= len(self.waypoints)
self.publish_migration_setpoint()
self.publish_migration_markers()
class ReleaseController():
def __init__(self, grasp_params_dict, w_ext_params_dict):
self.grasp_type = grasp_params_dict['grasp_type']
self.release_is_active = False
self.w_ext = {'lin': np.zeros(3), 'ang': np.zeros(3), 't': 0.0}
self.w_ext_d = {'lin': np.zeros(3), 'ang': np.zeros(3), 't': 0.0}
self.w_ext_rel = {'lin': np.zeros(3), 'ang': np.zeros(3), 't': 0.0}
self.w_ext_rel_d = {'lin': np.zeros(3), 'ang': np.zeros(3), 't': 0.0}
self.lp_filters = {
'w_ext': {
'lin':
Discrete_Low_Pass_VariableStep(dim=3,
fc=w_ext_params_dict['fc']),
'ang':
Discrete_Low_Pass_VariableStep(dim=3,
fc=w_ext_params_dict['fc'])
},
'w_ext_rel': {
'lin':
Discrete_Low_Pass_VariableStep(dim=3,
fc=w_ext_params_dict['fc']),
'ang':
Discrete_Low_Pass_VariableStep(dim=3,
fc=w_ext_params_dict['fc'])
}
}
self.allowed_action_state_list = ('GRASP_ACTIVE', 'GRASP_NOT_ACTIVE')
self.action_state = 'GRASP_NOT_ACTIVE'
self.allowed_system_state_list = ('WAIT', 'READY')
self.system_state = 'WAIT'
self.allowed_grasp_state = {'CLOSED', 'OPEN'}
self.grasp_state = 'OPEN'
# Publishers
self.DMP_goal_pub = rospy.Publisher('/DMP/target_goal',
Pose,
queue_size=5)
self.autograsp_pub = rospy.Publisher('/autograsp_controller/command',
AutoGrasp,
queue_size=5)
self.optoforce_reset_pub = rospy.Publisher('/reset_OptoForce_absolute',
Bool,
queue_size=5)
self.action_state_pub = rospy.Publisher('/state_machine/action_state',
String,
queue_size=5)
self.system_state_pub = rospy.Publisher('/state_machine/system_state',
String,
queue_size=5)
# Test publisher, can be used to test functions
self.test_publisher = rospy.Publisher('/release_controller/test',
Point,
queue_size=5)
# Subscribers
self.external_wrench_sub = rospy.Subscriber(
'/DMP/external_wrench',
ExternalWrench,
callback=self.update_external_wrench,
queue_size=5)
self.action_state_sub = rospy.Subscriber(
'/state_machine/action_state',
String,
callback=self.update_action_state,
queue_size=5)
self.system_state_sub = rospy.Subscriber(
'/state_machine/system_state',
String,
callback=self.update_system_state,
queue_size=5)
# Dunamic reconfigure clients
self.ExtEffDynClient = DynRClient(
name='/DMP_controller/set_parameters_ext_effects', timeout=None)
self.dyn_srv = Server(ReleaseControllerConfig,
self.dyn_reconfigure_callback)
def update_external_wrench(self, external_wrench_msg):
# Signal is filtered with the low-pass discrete filters defined by self.lp_filters (that also compute the derivative of the fltered signal)
t = external_wrench_msg.time.data
dt = t - self.w_ext['t']
self.w_ext['t'] = t
self.w_ext_d['t'] = t
self.w_ext_rel['t'] = t
self.w_ext_rel_d['t'] = t
if (dt > 100.0):
return
self.w_ext['lin'], self.w_ext_d['lin'] = self.lp_filters['w_ext'][
'lin'].filter(
dt,
np.array([
external_wrench_msg.absolute.force.x,
external_wrench_msg.absolute.force.y,
external_wrench_msg.absolute.force.z
]))
self.w_ext['ang'], self.w_ext_d['ang'] = self.lp_filters['w_ext'][
'ang'].filter(
dt,
np.array([
external_wrench_msg.absolute.torque.x,
external_wrench_msg.absolute.torque.y,
external_wrench_msg.absolute.torque.z
]))
self.w_ext_rel['lin'], self.w_ext_rel_d['lin'] = self.lp_filters[
'w_ext_rel']['lin'].filter(
dt,
np.array([
external_wrench_msg.relative.force.x,
external_wrench_msg.relative.force.y,
external_wrench_msg.relative.force.z
]))
self.w_ext_rel['ang'], self.w_ext_rel_d['ang'] = self.lp_filters[
'w_ext_rel']['ang'].filter(
dt,
np.array([
external_wrench_msg.relative.torque.x,
external_wrench_msg.relative.torque.y,
external_wrench_msg.relative.torque.z
]))
# msg = Point()
# msg.x = self.w_ext['lin'][0]
# msg.y = self.w_ext['lin'][1]
# msg.z = self.w_ext['lin'][2]
# self.test_publisher.publish(msg)
def update_action_state(self, string_msg):
if string_msg.data in self.allowed_action_state_list:
self.action_state = string_msg.data
else:
rospy.logwarn(
'update_action_state: \'%s\' value not allowed for action state'
% (string_msg.data))
def update_system_state(self, string_msg):
if string_msg.data in self.allowed_system_state_list:
self.system_state = string_msg.data
else:
rospy.logwarn(
'update_system_state: \'%s\' value not allowed for system state'
% (string_msg.data))
def reset_OptoForce_absolute(self):
msg = Bool()
msg.data = True
self.optoforce_reset_pub.publish(msg)
def set_virtual_compliance(self, **kwargs):
new_params = kwargs
self.ExtEffDynClient.update_configuration(new_params)
def send_grasp_pose(self, grasp_type, grasp_step, grasp_force):
if (grasp_step > 100) or (grasp_force > 255):
rospy.logwarn(
"send_grasp_pose() error: grasp_step>100 or grasp_force>255. Not publishing"
)
return
if not (grasp_type in ['RLX', 'LAT', 'PIN', 'TRI', 'CYL']):
rospy.logwarn(
"send_grasp_pose() error: selected grasp_type (\'%s\') doesn't exist. Not publishing"
% (grasp_type))
return
msg = AutoGrasp()
msg.grasp_type = grasp_type
msg.grasp_step = int(grasp_step)
msg.grasp_force = int(grasp_force)
self.autograsp_pub.publish(msg)
def set_grasp_state(self, state):
if not (state in self.allowed_grasp_state):
rospy.logwarn(
'set_grasp_state: selected state (\'%s\') is not allowed' %
(state))
self.grasp_state = state
def release(self):
Kf_ext_z_old = self.ExtEffDynClient.get_configuration()['Kf_ext_z']
# Remove z-axis compliance and coupling effect?
self.set_virtual_compliance(Kf_ext_z=0.0)
# Open hand
self.send_grasp_pose(self.grasp_type, 0, 200)
# state_msg = String()
# state_msg.data = 'GRASP_NOT'
# self.system_state_pub.publish(state_msg)
# Sleep for 0.5 seconds
rospy.sleep(rospy.Duration(secs=0, nsecs=5e8))
self.set_grasp_state('OPEN')
# Reset OptoForce on current value (object should have already been taken by the human)
self.reset_OptoForce_absolute()
# Rectivate z-axis compliance with the same value as before
self.set_virtual_compliance(Kf_ext_z=Kf_ext_z_old)
def grab(self):
Kf_ext_z_old = self.ExtEffDynClient.get_configuration()['Kf_ext_z']
# Remove z-axis compliance and coupling effect?
self.set_virtual_compliance(Kf_ext_z=0.0)
# Open hand
self.send_grasp_pose(self.grasp_type, 80, 200)
rospy.sleep(rospy.Duration(secs=2, nsecs=0))
self.set_grasp_state('CLOSED')
# Reset OptoForce on current value (object should have already been taken by the human)
self.reset_OptoForce_absolute()
# Rectivate z-axis compliance with the same value as before
self.set_virtual_compliance(Kf_ext_z=Kf_ext_z_old)
def check_release_condition(self):
return (np.linalg.norm(self.w_ext['lin']) > self.F_TRIGGER) and (
self.action_state
== 'GRASP_ACTIVE') and (self.grasp_state
== 'CLOSED') and (self.system_state
== 'READY')
def check_closing_condition(self):
return (np.linalg.norm(self.w_ext['lin']) > self.F_TRIGGER) and (
self.action_state == 'GRASP_ACTIVE') and (
self.grasp_state == 'OPEN') and (self.system_state == 'READY')
def publish_DMP_goal(self, pose):
msg = Pose()
msg.position.x = pose['lin'][0]
msg.position.y = pose['lin'][1]
msg.position.z = pose['lin'][2]
msg.orientation.w = pose['ang'].w
msg.orientation.x = pose['ang'].x
msg.orientation.y = pose['ang'].y
msg.orientation.z = pose['ang'].z
self.DMP_goal_pub.publish(msg)
def dyn_reconfigure_callback(self, config, level):
self.F_TRIGGER = config['F_TRIGGER']
return config
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
