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 MockNavigation():
def __init__(self, move_base_topic):
self.robot_pose_ = PoseStamped()
self.listener = tf.TransformListener()
self.navigation_succedes = True
self.reply = False
self.preempted = 0
self.moves_base = False
self.target_position = Point()
self.target_orientation = Quaternion()
self.move_base_as_ = SimpleActionServer(
move_base_topic,
MoveBaseAction,
execute_cb = self.move_base_cb,
auto_start = False)
self.move_base_as_.start()
def __del__(self):
self.move_base_as_.__del__()
def move_base_cb(self, goal):
rospy.loginfo('move_base_cb')
self.target_position = goal.target_pose.pose.position
self.target_orientation = goal.target_pose.pose.orientation
self.moves_base = True
while not self.reply:
rospy.sleep(0.2)
(trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
self.robot_pose_.pose.position.x = trans[0]
self.robot_pose_.pose.position.y = trans[1]
feedback = MoveBaseFeedback()
feedback.base_position.pose.position.x = \
self.robot_pose_.pose.position.x
feedback.base_position.pose.position.y = \
self.robot_pose_.pose.position.y
self.move_base_as_.publish_feedback(feedback)
if self.move_base_as_.is_preempt_requested():
self.preempted += 1
rospy.loginfo("Preempted!")
self.moves_base = False
self.move_base_as_.set_preempted()
return None
if self.move_base_as_.is_new_goal_available():
self.preempted += 1
self.move_base_cb(self.move_base_as_.accept_new_goal())
return None
else:
result = MoveBaseResult()
self.reply = False
self.preempted = 0
self.moves_base = False
self.target_position = Point()
self.target_orientation = Quaternion()
if self.navigation_succedes:
self.move_base_as_.set_succeeded(result)
else:
self.move_base_as_.set_aborted(result)
class move_base_fake_node:
def __init__(self):
self.action_server = SimpleActionServer(
'move_base',
MoveBaseAction,
execute_cb=self.execute_callback,
auto_start=False
) # Simple action server will pretend to be move_base
# Movement goal state
self.goal = None # Is a goal set
self.valid_goal = False # Is this a valid goal
self.current_goal_started = False # Has the goal been started (i.e. have we told our Bug algorithm to use this point and start)
self.current_goal_complete = False # Has the Bug algorithm told us it completed
self.position = None # move_base feedback reports the current direction
## TO DO!!
# Need a service provided by this node or something for the Bug algorithm to tell us it is done
# Bug service to start and stop Bug algorithm
# Bug service to set a new goal in Bug algorithm
# rospy.wait_for_service()
# rospy.wait_for_service()
self.subscriber_odometry = rospy.Subscriber(
'odom/', Odometry, self.callback_odometry
) # We need to read the robots current point for the feedback
self.subscriber_simple_goal = rospy.Subscriber(
'/move_base_simple/goal/', PoseStamped, self.callback_simple_goal
) # Our return goal is done with /move_base_simple/goal/
self.goal_pub = rospy.Publisher(
'/move_base/goal/', MoveBaseActionGoal,
queue_size=10) # /move_base_simple/goal/ gets published here
self.action_server.start()
def execute_callback(self, move_base_goal):
self.goal = move_base_goal.target_pose.pose # Set the provided goal as the current goal
rospy.logdebug('[Move Base Fake] Execute Callback: {}'.format(
str(self.goal.position)))
self.valid_goal = True # Assume it is valid
self.current_goal_started = False # It hasnt started yet
self.current_goal_complete = False # It hasnt been completed
r = rospy.Rate(1)
while not rospy.is_shutdown():
# Always start by checking if there is a new goal that preempts the current one
if self.action_server.is_preempt_requested():
## TO DO!!
# Tell Bug algorithm to stop before changing or stopping goal
if self.action_server.is_new_goal_available():
new_goal = self.action_server.accept_new_goal(
) # There is a new goal
rospy.logdebug('[Move Base Fake] New Goal: {}'.format(
str(self.goal.position)))
self.goal = new_goal.target_pose.pose # Set the provided goal as the current goal
self.valid_goal = True # Assume it is valid
self.current_goal_started = False # It hasnt started yet
self.current_goal_complete = False # It hasnt been completed
else:
self.action_server.set_preempted(
) # No new goal, we've just been told to stop
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
return
# Start goal
if self.valid_goal and not self.current_goal_started:
rospy.logdebug('[Move Base Fake] Starting Goal')
## TO DO !!
# Call the Bug services/topics etc to tell Bug algorithm new target point and then to start
self.current_goal_started = True # Only start once
# Feedback ever loop just reports current location
feedback = MoveBaseFeedback()
feedback.base_position.pose.position = self.position
self.action_server.publish_feedback(feedback)
# Completed is set in a callback that you need to link to a service or subscriber
if self.current_goal_complete:
rospy.logdebug('[Move Base Fake] Finishing Goal')
## TO DO!!
# Tell Bug algorithm to stop before changing or stopping goal
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
self.action_server.set_succeeded(
MoveBaseResult(), 'Goal reached') # Send success message
return
r.sleep()
# Shutdown
rospy.logdebug('[Move Base Fake] Shutting Down')
## TO DO!!
# Tell Bug algorithm to stop before changing or stopping goal
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
# you need to connect this to something being called/published from the Bug algorithm
def callback_complete(self, success):
# TO DO!!
# Implement some kind of service or subscriber so the Bug algorithm can tell this node it is complete
self.current_goal_complete = success.data
def callback_odometry(self, odom):
self.position = odom.pose.pose.position
# Simple goals get republished to the correct topic
def callback_simple_goal(self, goal):
rospy.logdebug('[Move Base Fake] Simple Goal: {}'.format(
str(goal.pose.position)))
action_goal = MoveBaseActionGoal()
action_goal.header.stamp = rospy.Time.now()
action_goal.goal.target_pose = goal
self.goal_pub.publish(action_goal)
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)
