class AveragingSVR2(object):
def __init__(self):
self._action = SimpleActionServer('averaging',
AveragingAction,
auto_start=False)
self._action.register_preempt_callback(self.preempt_cb)
self._action.register_goal_callback(self.goal_cb)
self.reset_numbers()
rospy.Subscriber('number', Float32, self.execute_loop)
self._action.start()
def std_dev(self, lst):
ave = sum(lst) / len(lst)
return sum([x * x for x in lst]) / len(lst) - ave**2
def goal_cb(self):
self._goal = self._action.accept_new_goal()
rospy.loginfo('goal callback %s' % (self._goal))
def preempt_cb(self):
rospy.loginfo('preempt callback')
self.reset_numbers()
self._action.set_preempted(text='message for preempt')
def reset_numbers(self):
self._samples = []
def execute_loop(self, msg):
if (not self._action.is_active()):
return
self._samples.append(msg.data)
feedback = AveragingAction().action_feedback.feedback
feedback.sample = len(self._samples)
feedback.data = msg.data
feedback.mean = sum(self._samples) / len(self._samples)
feedback.std_dev = self.std_dev(self._samples)
self._action.publish_feedback(feedback)
## sending result
if (len(self._samples) >= self._goal.samples):
result = AveragingAction().action_result.result
result.mean = sum(self._samples) / len(self._samples)
result.std_dev = self.std_dev(self._samples)
rospy.loginfo('result: %s' % (result))
self.reset_numbers()
if (result.mean > 0.5):
self._action.set_succeeded(result=result,
text='message for succeeded')
else:
self._action.set_aborted(result=result,
text='message for aborted')
class MotionService(object):
def __init__(self):
rospy.init_node('motion_service')
# Load config
config_loader = RobotConfigLoader()
try:
robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config')
except KeyError:
rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for '
'Thor Mang.')
robot_config_name = 'thor'
config_loader.load_xml_by_name(robot_config_name + '_config.xml')
# Create publisher for first target
if len(config_loader.targets) > 0:
self._motion_publisher = MotionPublisher(
config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix)
else:
rospy.logerr('Robot config needs to contain at least one valid target.')
self._motion_data = MotionData(config_loader.robot_config)
# Subscriber to start motions via topic
self.motion_command_sub = rospy.Subscriber('motion_command', ExecuteMotionCommand, self.motion_command_request_cb)
# Create action server
self._action_server = SimpleActionServer(rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False)
self._action_server.register_goal_callback(self._execute_motion_goal)
self._action_server.register_preempt_callback(self._preempt_motion_goal)
self._preempted = False
def _execute_motion_goal(self):
goal = self._action_server.accept_new_goal()
# Check if motion exists
if goal.motion_name not in self._motion_data:
result = ExecuteMotionResult()
result.error_code = [ExecuteMotionResult.MOTION_UNKNOWN]
result.error_string = "The requested motion is unknown."
self._action_server.set_aborted(result)
return
# check if a valid time_factor is set, otherwise default to 1.0
if goal.time_factor <= 0.0:
goal.time_factor = 1.0
self._preempted = False
self._motion_publisher.publish_motion(self._motion_data[goal.motion_name], goal.time_factor, self._trajectory_finished)
print '[MotionService] New action goal received.'
def _preempt_motion_goal(self):
self._motion_publisher.stop_motion()
print '[MotionService] Preempting goal.'
self._preempted = True
def _trajectory_finished(self, error_codes, error_groups):
result = ExecuteMotionResult()
result.error_code = error_codes
error_string = ""
for error_code, error_group in zip(error_codes, error_groups):
error_string += error_group + ': ' + str(error_code) + '\n'
result.error_string = error_string
if self._preempted:
self._action_server.set_preempted(result)
else:
self._action_server.set_succeeded(result)
print '[MotionService] Trajectory finished with error code: \n', error_string
def _execute_motion(self, request):
response = ExecuteMotionResponse()
# check if a motion with this name exists
if request.motion_name not in self._motion_data:
print '[MotionService] Unknown motion name: "%s"' % request.motion_name
response.ok = False
response.finish_time.data = rospy.Time.now()
return response
# check if a valid time_factor is set, otherwise default to 1.0
if request.time_factor <= 0.0:
request.time_factor = 1.0
# find the duration of the requested motion
motion_duration = 0.0
for poses in self._motion_data[request.motion_name].values():
if len(poses) > 0:
endtime = poses[-1]['starttime'] + poses[-1]['duration']
motion_duration = max(motion_duration, endtime)
motion_duration *= request.time_factor
# execute motion
print '[MotionService] Executing motion: "%s" (time factor: %.3f, duration %.2fs)' % (request.motion_name, request.time_factor, motion_duration)
self._motion_publisher.publish_motion(self._motion_data[request.motion_name], request.time_factor)
# reply with ok and the finish_time of this motion
response.ok = True
response.finish_time.data = rospy.Time.now() + rospy.Duration.from_sec(motion_duration)
return response
def start_server(self):
rospy.Service(rospy.get_name() + '/execute_motion', ExecuteMotion, self._execute_motion)
self._action_server.start()
print '[MotionService] Waiting for calls...'
rospy.spin()
def motion_command_request_cb(self, command):
print "[MotionService] Initiate motion command via topic ..."
request = ExecuteMotion()
request.motion_name = command.motion_name
request.time_factor = command.time_factor
self._execute_motion(request)
print "[MotionService] Motion command complete"
class ActionPerformer(Node, ABC):
def __init__(self, name: str):
super().__init__(name, "action")
self.name = name
# Service for action point computation
self._action_point_server = Service(get_action_point_service(name),
ComputeActionPoint,
self._compute_action_point)
# Action server for this action
self._action_server = SimpleActionServer(get_action_server(name),
PerformAction,
auto_start=False)
self._action_server.register_goal_callback(self._on_goal)
self._action_server.register_preempt_callback(self._on_preempt)
self._action_server.start()
# Function managing the action
def returns(self, state=ActionStatus.DONE):
'''
Function to call when the action is interrupted
with a given state
'''
# Create result message
result = PerformResult()
result.state = state
# Send back to client
self._action_server.set_succeeded(result)
# Hidden handlers
def _compute_action_point(
self,
request: ComputeActionPointRequest) -> ComputeActionPointResponse:
# Extract data from request and call performer's function
result: ActionPoint = self.compute_action_point(
Argumentable().from_list(request.args), request.robot_pos)
response = ComputeActionPointResponse()
response.action_point = result
return response
def _on_goal(self):
if self._action_server.is_new_goal_available():
goal: PerformGoal = self._action_server.accept_new_goal()
# run action with given args
self.start(Argumentable().from_list(goal.arguments))
def _on_preempt(self):
self.cancel()
self._action_server.set_preempted()
# Function to be defined by inherited actions
@abstractmethod
def compute_action_point(self, args: Argumentable,
robot_pos: Pose2D) -> ActionPoint:
pass
@abstractmethod
def start(self, args: Argumentable):
pass
def cancel(self):
pass
class MotionService(object):
def __init__(self):
rospy.init_node('motion_service')
# Load config
config_loader = RobotConfigLoader()
try:
robot_config_name = rospy.get_param(rospy.get_name() +
'/robot_config')
except KeyError:
rospy.logwarn('Could not find robot config param in /' +
rospy.get_name +
'/robot_config. Using default config for '
'Thor Mang.')
robot_config_name = 'thor'
config_loader.load_xml_by_name(robot_config_name + '_config.xml')
# Create publisher for first target
if len(config_loader.targets) > 0:
self._motion_publisher = MotionPublisher(
config_loader.robot_config,
config_loader.targets[0].joint_state_topic,
config_loader.targets[0].publisher_prefix)
else:
rospy.logerr(
'Robot config needs to contain at least one valid target.')
self._motion_data = MotionData(config_loader.robot_config)
# Subscriber to start motions via topic
self.motion_command_sub = rospy.Subscriber(
'motion_command', ExecuteMotionCommand,
self.motion_command_request_cb)
# Create action server
self._action_server = SimpleActionServer(
rospy.get_name() + '/motion_goal', ExecuteMotionAction, None,
False)
self._action_server.register_goal_callback(self._execute_motion_goal)
self._action_server.register_preempt_callback(
self._preempt_motion_goal)
self._preempted = False
def _execute_motion_goal(self):
goal = self._action_server.accept_new_goal()
# Check if motion exists
if goal.motion_name not in self._motion_data:
result = ExecuteMotionResult()
result.error_code = [ExecuteMotionResult.MOTION_UNKNOWN]
result.error_string = "The requested motion is unknown."
self._action_server.set_aborted(result)
return
# check if a valid time_factor is set, otherwise default to 1.0
if goal.time_factor <= 0.0:
goal.time_factor = 1.0
self._preempted = False
self._motion_publisher.publish_motion(
self._motion_data[goal.motion_name], goal.time_factor,
self._trajectory_finished)
print '[MotionService] New action goal received.'
def _preempt_motion_goal(self):
self._motion_publisher.stop_motion()
print '[MotionService] Preempting goal.'
self._preempted = True
def _trajectory_finished(self, error_codes, error_groups):
result = ExecuteMotionResult()
result.error_code = error_codes
error_string = ""
for error_code, error_group in zip(error_codes, error_groups):
error_string += error_group + ': ' + str(error_code) + '\n'
result.error_string = error_string
if self._preempted:
self._action_server.set_preempted(result)
else:
self._action_server.set_succeeded(result)
print '[MotionService] Trajectory finished with error code: \n', error_string
def _execute_motion(self, request):
response = ExecuteMotionResponse()
# check if a motion with this name exists
if request.motion_name not in self._motion_data:
print '[MotionService] Unknown motion name: "%s"' % request.motion_name
response.ok = False
response.finish_time.data = rospy.Time.now()
return response
# check if a valid time_factor is set, otherwise default to 1.0
if request.time_factor <= 0.0:
request.time_factor = 1.0
# find the duration of the requested motion
motion_duration = 0.0
for poses in self._motion_data[request.motion_name].values():
if len(poses) > 0:
endtime = poses[-1]['starttime'] + poses[-1]['duration']
motion_duration = max(motion_duration, endtime)
motion_duration *= request.time_factor
# execute motion
print '[MotionService] Executing motion: "%s" (time factor: %.3f, duration %.2fs)' % (
request.motion_name, request.time_factor, motion_duration)
self._motion_publisher.publish_motion(
self._motion_data[request.motion_name], request.time_factor)
# reply with ok and the finish_time of this motion
response.ok = True
response.finish_time.data = rospy.Time.now() + rospy.Duration.from_sec(
motion_duration)
return response
def start_server(self):
rospy.Service(rospy.get_name() + '/execute_motion', ExecuteMotion,
self._execute_motion)
self._action_server.start()
print '[MotionService] Waiting for calls...'
rospy.spin()
def motion_command_request_cb(self, command):
print "[MotionService] Initiate motion command via topic ..."
request = ExecuteMotion()
request.motion_name = command.motion_name
request.time_factor = command.time_factor
self._execute_motion(request)
print "[MotionService] Motion command complete"
class Server(object):
def __init__(self, name):
rospy.loginfo("Starting '{test}'.".format(test=name))
self._as = SimpleActionServer(name, WayPointNavAction, auto_start=False)
self._as.register_goal_callback(self.execute_cb)
self._as.register_preempt_callback(self.preempt_cb)
self.listener = tf.TransformListener()
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
self.sub = None
self._as.start()
rospy.loginfo("Started '{test}'.".format(test=name))
def execute_cb(self):
self.goal = self._as.accept_new_goal()
print self.goal
self.sub = rospy.Subscriber("/orb_slam/pose", PoseStamped, self.pose_cb)
def preempt_cb(self, *args):
self.sub.unregister()
self.sub = None
t = Twist()
self.pub.publish(t)
self._as.set_preempted(WayPointNavResult())
def pose_cb(self, msg):
dist = self.get_dist(self.goal.goal, msg)
t = Twist()
new_goal = self._transform(self.goal.goal, "camera_pose")
print new_goal
print "Dist", dist
theta = self.get_theta(new_goal.pose)
print "Theta", theta
if dist < 0.02:
self._as.set_succeeded(WayPointNavResult())
self.sub.unregister()
self.sub = None
t.linear.x = 0.0
t.angular.z = 0.0
else:
t.linear.x = 0.03
t.angular.z = theta * .6
self.pub.publish(t)
def get_dist(self, pose1, pose2):
return np.sqrt(np.power(pose1.pose.position.x-pose2.pose.position.x, 2)+np.power(pose1.pose.position.y-pose2.pose.position.y, 2))
def _transform(self, msg, target_frame):
if msg.header.frame_id != target_frame:
try:
t = self.listener.getLatestCommonTime(target_frame, msg.header.frame_id)
msg.header.stamp = t
return self.listener.transformPose(target_frame, msg)
except (tf.Exception, tf.LookupException, tf.ConnectivityException) as ex:
rospy.logwarn(ex)
return None
else:
return msg
def get_theta(self, pose):
return np.arctan2(pose.position.y, pose.position.x)
