class PipolFollower():
def __init__(self):
rospy.loginfo("Creating Pipol follower AS: '" + PIPOL_FOLLOWER_AS + "'")
self._as = SimpleActionServer(PIPOL_FOLLOWER_AS, PipolFollowAction,
execute_cb = self.execute_cb,
preempt_callback = self.preempt_cb,
auto_start = False)
rospy.loginfo("Starting " + PIPOL_FOLLOWER_AS)
self._as.start()
def execute_cb(self, goal):
print "goal is: " + str(goal)
# helper variables
success = True
# start executing the action
for i in xrange(1, goal.order):
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
break
self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
# publish the feedback
self._as.publish_feedback(self._feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep()
if success:
self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
class MockExplorer():
def __init__(self, exploration_topic):
self.robot_pose_ = PoseStamped()
self.listener = tf.TransformListener()
self.navigation_succedes = True
self.reply = False
self.preempted = 0
self.entered_exploration = False
self.do_exploration_as_ = SimpleActionServer(
exploration_topic,
DoExplorationAction,
execute_cb=self.do_exploration_cb,
auto_start=False)
self.do_exploration_as_.start()
def __del__(self):
self.do_exploration_as_.__del__()
def do_exploration_cb(self, goal):
rospy.loginfo('do_exploration_cb')
self.entered_exploration = 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 = DoExplorationFeedback()
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.do_exploration_as_.publish_feedback(feedback)
if self.do_exploration_as_.is_preempt_requested():
self.preempted += 1
rospy.loginfo("Preempted!")
self.entered_exploration = False
self.do_exploration_as_.set_preempted(DoExplorationResult())
return None
else:
result = DoExplorationResult()
self.reply = False
self.preempted = 0
self.entered_exploration = False
if self.navigation_succedes:
self.do_exploration_as_.set_succeded(result)
else:
self.do_exploration_as_.set_aborted(result)
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 FibonacciActionServer(object):
# create messages that are used to publish feedback/result
feedback = FibonacciFeedback()
result = FibonacciResult()
def __init__(self, name):
self.action_name = name
self.action_server = SimpleActionServer(self.action_name,
FibonacciAction,
execute_cb=self.execute_cb,
auto_start=False)
self.action_server.start()
def execute_cb(self, goal):
# helper variables
r = rospy.Rate(1)
success = True
# append the seeds for the fibonacci sequence
self.feedback.sequence = []
self.feedback.sequence.append(0)
self.feedback.sequence.append(1)
# publish info to the console for the user
rospy.loginfo(
'%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i'
% (self.action_name, goal.order, self.feedback.sequence[0],
self.feedback.sequence[1]))
# start executing the action
for i in range(1, goal.order):
# check that preempt has not been requested by the client
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self.action_name)
self.action_server.set_preempted()
success = False
break
self.feedback.sequence.append(self.feedback.sequence[i] +
self.feedback.sequence[i - 1])
# publish the feedback
rospy.loginfo('publishing feedback ...')
self.action_server.publish_feedback(self.feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep()
if success:
self.result.sequence = self.feedback.sequence
rospy.loginfo('%s: Succeeded' % self.action_name)
self.action_server.set_succeeded(self.result)
class SynchronizedSimpleActionServer(object):
def __init__(self, namespace, action_spec, execute_cb):
self.goal_service = rospy.Service(namespace + '/get_goal_from_id',
GetTaskFromID, self.task_id_cb)
self.server = SimpleActionServer(namespace,
action_spec,
execute_cb,
auto_start=False)
self.server.start()
def task_id_cb(self, request):
idx = request.task_id
current_goal = self.server.current_goal
if idx == current_goal.goal_id.id:
return GetTaskFromIDResponse(current_goal.get_goal())
return GetTaskFromIDResponse()
def is_preempt_requested(self):
return self.server.is_preempt_requested()
def set_preempted(self):
return self.server.set_preempted()
def set_succeeded(self, result):
return self.server.set_succeeded(result)
def publish_feedback(self, feedback):
return self.server.publish_feedback(feedback)
class TestServerClass():
def __init__(self):
self.smach_action_server = SimpleActionServer('test_smach_action_server',testAction,
execute_cb=self.execute_cb,auto_start=False)
self.smach_action_server.start()
def execute_cb(self,goal):
sm = getStateMachine()
sm.userdata.action_goal = goal
smach_thread = Thread(target=sm.execute)
smach_thread.start()
while(smach_thread.is_alive()):
self.smach_action_server.publish_feedback(testFeedback(str(sm.get_active_states())))
rospy.loginfo(sm.get_active_states())
rospy.sleep(rospy.Duration(1.0))
self.smach_action_server.set_succeeded(testResult("The Task Got Completed"))
class AveragingSVR(object):
def __init__(self):
self._action = SimpleActionServer('averaging',
AveragingAction,
execute_cb=self.execute_cb,
auto_start=False)
self._action.register_preempt_callback(self.preempt_cb)
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 preempt_cb(self):
rospy.loginfo('preempt callback')
self._action.set_preempted(text='message for preempt')
def execute_cb(self, goal):
rospy.loginfo('execute callback: %s' % (goal))
feedback = AveragingAction().action_feedback.feedback
result = AveragingAction().action_result.result
## execute loop
rate = rospy.Rate(1 / (0.01 + 0.99 * random.random()))
samples = []
for i in range(goal.samples):
sample = random.random()
samples.append(sample)
feedback.sample = i
feedback.data = sample
feedback.mean = sum(samples) / len(samples)
feedback.std_dev = self.std_dev(samples)
self._action.publish_feedback(feedback)
rate.sleep()
if (not self._action.is_active()):
rospy.loginfo('not active')
return
## sending result
result.mean = sum(samples) / len(samples)
result.std_dev = self.std_dev(samples)
rospy.loginfo('result: %s' % (result))
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 fibonacci_server:
def __init__(self, name):
self.name = name
self.feedback = FibonacciFeedback()
self.result = FibonacciResult()
self.action_server = SimpleActionServer(
self.name,
FibonacciAction,
execute_cb=self.execute_callback,
auto_start=False)
self.action_server.start()
def execute_callback(self, goal):
rate = rospy.Rate(1)
success = True
self.feedback.sequence[:] = []
self.feedback.sequence.append(0)
self.feedback.sequence.append(1)
rospy.loginfo(
'[{}] Executing, creating fibonacci sequence of order {} with seeds {}, {}'
.format(self.name, goal.order, self.feedback.sequence[0],
self.feedback.sequence[1]))
for i in range(1, goal.order):
if self.action_server.is_preempt_requested() or rospy.is_shutdown(
):
rospy.loginfo('[{}] Prempeted'.format(self.name))
success = False
self.action_server.set_preempted()
break
self.feedback.sequence.append(self.feedback.sequence[i - 1] +
self.feedback.sequence[i])
self.action_server.publish_feedback(self.feedback)
rate.sleep()
if success:
self.result.sequence = self.feedback.sequence
rospy.loginfo('[{}] Succeeded'.format(self.name))
self.action_server.set_succeeded(self.result)
class PipolFollower():
def __init__(self):
rospy.loginfo("Creating Pipol follower AS: '" + PIPOL_FOLLOWER_AS +
"'")
self._as = SimpleActionServer(PIPOL_FOLLOWER_AS,
PipolFollowAction,
execute_cb=self.execute_cb,
preempt_callback=self.preempt_cb,
auto_start=False)
rospy.loginfo("Starting " + PIPOL_FOLLOWER_AS)
self._as.start()
def execute_cb(self, goal):
print "goal is: " + str(goal)
# helper variables
success = True
# start executing the action
for i in xrange(1, goal.order):
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
break
self._feedback.sequence.append(self._feedback.sequence[i] +
self._feedback.sequence[i - 1])
# publish the feedback
self._as.publish_feedback(self._feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep()
if success:
self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
class SmartArmGripperActionServer():
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 2 # Number of joints to manage
self.ERROR_THRESHOLD = 0.01 # Report success if error reaches below threshold
self.TIMEOUT_THRESHOLD = rospy.Duration(5.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME, anonymous=True)
# Initialize publisher & subscriber for left finger
self.left_finger_frame = 'arm_left_finger_link'
self.left_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.left_finger_pub = rospy.Publisher('finger_left_controller/command', Float64)
rospy.Subscriber('finger_left_controller/state', JointControllerState, self.read_left_finger)
rospy.wait_for_message('finger_left_controller/state', JointControllerState)
# Initialize publisher & subscriber for right finger
self.right_finger_frame = 'arm_right_finger_link'
self.right_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.right_finger_pub = rospy.Publisher('finger_right_controller/command', Float64)
rospy.Subscriber('finger_right_controller/state', JointControllerState, self.read_right_finger)
rospy.wait_for_message('finger_right_controller/state', JointControllerState)
# Initialize action server
self.result = SmartArmGripperResult()
self.feedback = SmartArmGripperFeedback()
self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
self.server = SimpleActionServer(NAME, SmartArmGripperAction, self.execute_callback)
# Reset gripper position
rospy.sleep(1)
self.reset_gripper_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
def reset_gripper_position(self):
self.left_finger_pub.publish(0.0)
self.right_finger_pub.publish(0.0)
rospy.sleep(5)
def read_left_finger(self, data):
self.left_finger = data
self.has_latest_left_finger = True
def read_right_finger(self, data):
self.right_finger = data
self.has_latest_right_finger = True
def wait_for_latest_controller_states(self, timeout):
self.has_latest_left_finger = False
self.has_latest_right_finger = False
r = rospy.Rate(100)
start = rospy.Time.now()
while (self.has_latest_left_finger == False or self.has_latest_right_finger == False) and \
(rospy.Time.now() - start < timeout):
r.sleep()
def execute_callback(self, goal):
r = rospy.Rate(100)
self.result.success = True
self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
rospy.loginfo("%s: Executing move gripper", NAME)
# Initialize target joints
target_joints = list()
for i in range(self.JOINTS_COUNT):
target_joints.append(0.0)
# Retrieve target joints from goal
if (len(goal.target_joints) > 0):
for i in range(min(len(goal.target_joints), len(target_joints))):
target_joints[i] = goal.target_joints[i]
else:
rospy.loginfo("%s: Aborted: Invalid Goal", NAME)
self.result.success = False
self.server.set_aborted()
return
# Publish goal to controllers
self.left_finger_pub.publish(target_joints[0])
self.right_finger_pub.publish(target_joints[1])
# Initialize loop variables
start_time = rospy.Time.now()
while (math.fabs(target_joints[0] - self.left_finger.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[1] - self.right_finger.process_value) > self.ERROR_THRESHOLD):
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
break
# Publish current gripper position as feedback
self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > self.TIMEOUT_THRESHOLD):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
break
r.sleep()
if (self.result.success):
rospy.loginfo("%s: Goal Completed", NAME)
self.wait_for_latest_controller_states(rospy.Duration(2.0))
self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
self.server.set_succeeded(self.result)
class OnlineBagger(object):
BAG_TOPIC = '/online_bagger/bag'
def __init__(self):
"""
Make dictionary of dequeues.
Subscribe to set of topics defined by the yaml file in directory
Stream topics up to a given stream time, dump oldest messages when limit is reached
Set up service to bag n seconds of data default to all of available data
"""
self.successful_subscription_count = 0 # successful subscriptions
self.iteration_count = 0 # number of iterations
self.streaming = True
self.get_params()
if len(self.subscriber_list) == 0:
rospy.logwarn('No topics selected to subscribe to. Closing.')
rospy.signal_shutdown('No topics to subscribe to')
return
self.make_dicts()
self._action_server = SimpleActionServer(OnlineBagger.BAG_TOPIC, BagOnlineAction,
execute_cb=self.start_bagging, auto_start=False)
self.subscribe_loop()
rospy.loginfo('Remaining Failed Topics: {}\n'.format(
self.get_subscriber_list(False)))
self._action_server.start()
def get_subscriber_list(self, status):
"""
Get string of all topics, if their subscribe status matches the input (True / False)
Outputs each topics: time_buffer(float in seconds), subscribe_statue(bool), topic(string)
"""
sub_list = ''
for topic in self.subscriber_list.keys():
if self.subscriber_list[topic][1] == status:
sub_list = sub_list + \
'\n{:13}, {}'.format(self.subscriber_list[topic], topic)
return sub_list
def get_params(self):
"""
Retrieve parameters from param server.
"""
self.dir = rospy.get_param('~bag_package_path', default=None)
# Handle bag directory for MIL bag script
if self.dir is None and 'BAG_DIR' in os.environ:
self.dir = os.environ['BAG_DIR']
self.stream_time = rospy.get_param(
'~stream_time', default=30) # seconds
self.resubscribe_period = rospy.get_param(
'~resubscribe_period', default=3.0) # seconds
self.dated_folder = rospy.get_param(
'~dated_folder', default=True) # bool
self.subscriber_list = {}
topics_param = rospy.get_param('~topics', default=[])
# Add topics from rosparam to subscribe list
for topic in topics_param:
time = topic[1] if len(topic) == 2 else self.stream_time
self.subscriber_list[topic[0]] = (time, False)
def add_unique_topic(topic):
if topic not in self.subscriber_list:
self.subscriber_list[topic] = (self.stream_time, False)
def add_env_var(var):
for topic in var.split():
add_unique_topic(topic)
# Add topics from MIL bag script environment variables
if 'BAG_ALWAYS' in os.environ:
add_env_var(os.environ['BAG_ALWAYS'])
for key in os.environ.keys():
if key[0:4] == 'bag_':
add_env_var(os.environ[key])
rospy.loginfo(
'Default stream_time: {} seconds'.format(self.stream_time))
rospy.loginfo('Bag Directory: {}'.format(self.dir))
def make_dicts(self):
"""
Make dictionary with sliceable deques() that will be filled with messages and time stamps.
Subscriber list contains all of the topics, their stream time and their subscription status:
A True status for a given topic corresponds to a successful subscription
A False status indicates a failed subscription.
Stream time for an individual topic is specified in seconds.
For Example:
self.subscriber_list[0:1] = [['/odom', 300 ,False], ['/absodom', 300, True]]
Indicates that '/odom' has not been subscribed to, but '/absodom' has been subscribed to
self.topic_messages is a dictionary of deques containing a list of tuples.
Dictionary Keys contain topic names
Each value for each topic contains a deque
Each deque contains a list of tuples
Each tuple contains a message and its associated time stamp
For example:
'/odom' is a potential topic name
self.topic_message['/odom'] is a deque
self.topic_message['/odom'][0] is the oldest message available in the deque
and its time stamp if available. It is a tuple with each element: (time_stamp, msg)
self.topic_message['/odom'][0][0] is the time stamp for the oldest message
self.topic_message['/odom'][0][1] is the message associated with the oldest topic
"""
self.topic_messages = {}
class SliceableDeque(deque):
def __getitem__(self, index):
if isinstance(index, slice):
return type(self)(itertools.islice(self, index.start,
index.stop, index.step))
return deque.__getitem__(self, index)
for topic in self.subscriber_list:
self.topic_messages[topic] = SliceableDeque(deque())
rospy.loginfo('Initial subscriber_list: {}'.format(
self.get_subscriber_list(False)))
def subscribe_loop(self):
"""
Continue to subscribe until at least one topic is successful,
then break out of loop and be called in the callback function to prevent the function
from locking up.
"""
self.resubscriber = None
i = 0
# if self.successful_subscription_count == 0 and not
# rospy.is_shutdown():
while self.successful_subscription_count == 0 and not rospy.is_shutdown():
self.subscribe()
rospy.sleep(0.1)
i = i + 1
if i % 1000 == 0:
rospy.logdebug('still subscribing!')
rospy.loginfo("Subscribed to {} of {} topics, will try again every {} seconds".format(
self.successful_subscription_count, len(self.subscriber_list), self.resubscribe_period))
self.resubscriber = rospy.Timer(rospy.Duration(
self.resubscribe_period), self.subscribe)
def subscribe(self, time_info=None):
"""
Subscribe to the topics defined in the yaml configuration file
Function checks subscription status True/False of each topic
if True: topic has already been sucessfully subscribed to
if False: topic still needs to be subscribed to and
subscriber will be run.
Each element in self.subscriber list is a list [topic, Bool]
where the Bool tells the current status of the subscriber (sucess/failure).
Return number of topics that failed subscription
"""
if self.successful_subscription_count == len(self.subscriber_list):
if self.resubscriber is not None:
self.resubscriber.shutdown()
rospy.loginfo(
'All topics subscribed too! Shutting down resubscriber')
for topic, (time, subscribed) in self.subscriber_list.items():
if not subscribed:
msg_class = rostopic.get_topic_class(topic)
if msg_class[1] is not None:
self.successful_subscription_count += 1
rospy.Subscriber(topic, msg_class[0],
lambda msg, _topic=topic: self.bagger_callback(msg, _topic))
self.subscriber_list[topic] = (time, True)
def get_topic_duration(self, topic):
"""
Return current time duration of topic
"""
return self.topic_messages[topic][-1][0] - self.topic_messages[topic][0][0]
def get_header_time(self, msg):
"""
Retrieve header time if available
"""
if hasattr(msg, 'header'):
return msg.header.stamp
else:
return rospy.get_rostime()
def get_time_index(self, topic, requested_seconds):
"""
Return the index for the time index for a topic at 'n' seconds from the end of the dequeue
For example, to bag the last 10 seconds of data, the index for 10 seconds back from the most
recent message can be obtained with this function.
The number of requested seconds should be the number of seoncds desired from
the end of deque. (ie. requested_seconds = 10 )
If the desired time length of the bag is greater than the available messages it will output a
message and return how ever many seconds of data are avaiable at the moment.
Seconds is of a number type (not a rospy.Time type) (ie. int, float)
"""
topic_duration = self.get_topic_duration(topic).to_sec()
if topic_duration == 0:
return 0
ratio = requested_seconds / topic_duration
index = int(self.get_topic_message_count(topic) * (1 - min(ratio, 1)))
return index
def bagger_callback(self, msg, topic):
"""
Streaming callback function, stops streaming during bagging process
also pops off msgs from dequeue if stream size is greater than specified stream_time
Stream, callback function does nothing if streaming is not active
"""
if not self.streaming:
return
self.iteration_count = self.iteration_count + 1
time = self.get_header_time(msg)
self.topic_messages[topic].append((time, msg))
time_diff = self.get_topic_duration(topic)
# verify streaming is popping off and recording topics
if self.iteration_count % 100 == 0:
rospy.logdebug("{} has {} messages spanning {} seconds".format(
topic, self.get_topic_message_count(topic), round(time_diff.to_sec(), 2)))
while time_diff > rospy.Duration(self.subscriber_list[topic][0]) and not rospy.is_shutdown():
self.topic_messages[topic].popleft()
time_diff = self.get_topic_duration(topic)
def get_topic_message_count(self, topic):
"""
Return number of messages available in a topic
"""
return len(self.topic_messages[topic])
def get_total_message_count(self):
"""
Returns total number of messages across all topics
"""
total_message_count = 0
for topic in self.topic_messages.keys():
total_message_count = total_message_count + \
self.get_topic_message_count(topic)
return total_message_count
def _get_default_filename(self):
return str(datetime.date.today()) + '-' + str(datetime.datetime.now().time())[0:8]
def get_bag_name(self, filename=''):
"""
Create ros bag save directory
If no bag name is provided, the current date/time is used as default.
"""
# If directory param is not set, default to $HOME/bags/<date>
default_dir = self.dir
if default_dir is None:
default_dir = os.path.join(os.environ['HOME'], 'bags')
# if dated folder param is set to True, append current date to
# directory
if self.dated_folder is True:
default_dir = os.path.join(default_dir, str(datetime.date.today()))
# Split filename from directory
bag_dir, bag_name = os.path.split(filename)
bag_dir = os.path.join(default_dir, bag_dir)
if not os.path.exists(bag_dir):
os.makedirs(bag_dir)
# Create default filename if only directory specified
if bag_name == '':
bag_name = self._get_default_filename()
# Make sure filename ends in .bag, add otherwise
if bag_name[-4:] != '.bag':
bag_name = bag_name + '.bag'
return os.path.join(bag_dir, bag_name)
def start_bagging(self, req):
"""
Dump all data in dictionary to bags, temporarily stops streaming
during the bagging process, resumes streaming when over.
If bagging is already false because of an active call to this service
"""
result = BagOnlineResult()
if self.streaming is False:
result.status = 'Bag Request came in while bagging, priority given to prior request'
result.success = False
self._action_server.set_aborted(result)
return
bag = None
try:
self.streaming = False
result.filename = self.get_bag_name(req.bag_name)
bag = rosbag.Bag(result.filename, 'w')
requested_seconds = req.bag_time
selected_topics = req.topics.split()
feedback = BagOnlineFeedback()
total_messages = 0
bag_topics = {}
for topic, (time, subscribed) in self.subscriber_list.iteritems():
if not subscribed:
continue
# Exclude topics that aren't in topics service argument
# If topics argument is empty string, include all topics
if len(selected_topics) > 0 and topic not in selected_topics:
continue
if len(self.topic_messages[topic]) == 0:
continue
if req.bag_time == 0:
index = 0
else:
index = self.get_time_index(topic, requested_seconds)
total_messages += len(self.topic_messages[topic][index:])
bag_topics[topic] = index
if total_messages == 0:
result.success = False
result.status = 'no messages'
self._action_server.set_aborted(result)
self.streaming = True
bag.close()
return
self._action_server.publish_feedback(feedback)
msg_inc = 0
for topic, index in bag_topics.iteritems():
for msgs in self.topic_messages[topic][index:]:
bag.write(topic, msgs[1], t=msgs[0])
if msg_inc % 50 == 0: # send feedback every 50 messages
feedback.progress = float(msg_inc) / total_messages
self._action_server.publish_feedback(feedback)
msg_inc += 1
# empty deque when done writing to bag
self.topic_messages[topic].clear()
feedback.progress = 1.0
self._action_server.publish_feedback(feedback)
bag.close()
except Exception as e:
result.success = False
result.status = 'Exception while writing bag: ' + str(e)
self._action_server.set_aborted(result)
self.streaming = True
if bag is not None:
bag.close()
return
rospy.loginfo('Bag written to {}'.format(result.filename))
result.success = True
self._action_server.set_succeeded(result)
self.streaming = True
class FaceTracker(object):
DIST_THRESH = 0.0
def __init__(self):
self.rate = rospy.Rate(2)
self.tf = tf.TransformListener()
self.eye_speed = 0.2
self.head_speed = 0.7
self.blender_frame = "blender"
self.default_position = [1, 0, 0] # Looking straight ahead 1 metre
self.last_position = None
# Publishers for making the face and eyes look at a point
self.face_target_pub = rospy.Publisher("/blender_api/set_face_target",
Target,
queue_size=1)
self.gaze_target_pub = rospy.Publisher("/blender_api/set_gaze_target",
Target,
queue_size=1)
# Gaze action server
self.action_srv = SimpleActionServer("/gaze_action",
GazeAction,
execute_cb=self.execute_cb,
auto_start=False)
self.action_srv.start()
def execute_cb(self, goal):
rospy.loginfo("Target goal received: " + str(goal))
target_frame = goal.target
while not rospy.is_shutdown(
) and not self.action_srv.is_preempt_requested(
) and self.action_srv.is_active():
if self.tf.frameExists(target_frame) and self.tf.frameExists(
self.blender_frame):
time = self.tf.getLatestCommonTime(target_frame,
self.blender_frame)
position, quaternion = self.tf.lookupTransform(
self.blender_frame, target_frame, time)
update_target = self.last_position is None
if self.last_position is not None:
dist = FaceTracker.distance(position, self.last_position)
update_target = dist > FaceTracker.DIST_THRESH
if update_target:
self.gaze_at_point(position)
self.action_srv.publish_feedback(GazeActionFeedback())
self.rate.sleep()
# If gaze has been cancelled then set default position
self.gaze_at_point(self.default_position)
@staticmethod
def distance(p1, p2):
return math.sqrt(
math.pow(p1[0] - p2[0], 2) + math.pow(p1[1] - p2[1], 2) +
math.pow(p1[2] - p2[2], 2))
def gaze_at_point(self, position):
x = position[0]
y = position[1]
z = position[2]
self.point_eyes_at_point(x, y, z, self.eye_speed)
self.face_toward_point(x, y, z, self.head_speed)
self.last_position = position
def point_eyes_at_point(self, x, y, z, speed):
""" Turn the robot's eyes towards the given target point
:param float x: metres forward
:param float y: metres to robots left
:param float z:
:param float speed:
:return: None
"""
msg = Target()
msg.x = x
msg.y = y
msg.z = z
msg.speed = speed
self.gaze_target_pub.publish(msg)
rospy.logdebug("published gaze_at(x={}, y={}, z={}, speed={})".format(
x, y, z, speed))
def face_toward_point(self, x, y, z, speed):
""" Turn the robot's face towards the given target point.
:param float x: metres forward
:param float y: metres to robots left
:param float z:
:param float speed:
:return: None
"""
msg = Target()
msg.x = x
msg.y = y
msg.z = z
msg.speed = speed
self.face_target_pub.publish(msg)
rospy.logdebug("published face_(x={}, y={}, z={}, speed={})".format(
x, y, z, speed))
class FakeTurtle:
def __init__(self):
# define the server, aut-start is set to false so we control when to
# start it
self.server = SimpleActionServer('move_turtle_action',
MoveTurtleAction,
execute_cb=self.action_cb,
auto_start=False)
# deine feedback msg object to be used for publishing feedback
self.feedback_msg = MoveTurtleFeedback()
# deine result msg object to be used for publishing result
self.result_msg = MoveTurtleResult()
self.server.start()
print("action server started ..")
self.rate = rospy.Rate(2)
def turtle_is_up_side_down(self):
# this turtle never goes up side down
# it can always move!
# (this is a dummy function, in real application you might be checking
# your robot status, for example the battery level,
# or motor temperature, etc..)
return False
def action_cb(self, goal):
print("moving turtle to pose: ", goal)
# Write here the logic to control turtle
# this "for" loop is just a demostration, in real application, you
# would be controlling the robot, publish velocity commands, read
# current pose and do some computation..
for _ in range(20):
print('executing goal')
# during execution of the goal, publish feedback msg to the client
self.server.publish_feedback(self.feedback_msg)
# always check if the client has sent a cancel msg. This way the
# task becomes preemtable
if self.server.is_preempt_requested():
# update goal status accordingly
self.server.set_preempted()
print('goal preempted')
# exit from the action_cb method
return None
# check if we need to abort for some reason. Here we abort if
# the node has been killed (ctrl+C) or if the turtle is laying on
# it's back and cannot move anyomre ;)
if rospy.is_shutdown() or self.turtle_is_up_side_down():
# update goal status accordingly
self.server.set_aborted()
print('Turtle cannot move sir! goal aborted')
return None
self.rate.sleep()
# if execution finished, set goal status to SUCCEED and publish the
# result msg
print('done')
self.server.set_succeeded(result=self.result_msg,
text="Mission completed")
class RobbieArmActionServer():
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 5 # Number of joints to manage
self.ERROR_THRESHOLD = 0.15 # Report success if error reaches below threshold
self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME + 'server', anonymous=True)
# Initialize publisher & subscriber for shoulder pan
self.shoulder_pan_frame = 'arm_shoulder_tilt_link'
self.shoulder_pan = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.shoulder_pan_pub = rospy.Publisher('shoulder_pan_controller/command', Float64)
rospy.Subscriber('shoulder_pan_controller/state', JointState, self.read_shoulder_pan)
rospy.wait_for_message('shoulder_pan_controller/state', JointState)
# Initialize publisher & subscriber for arm tilt
self.arm_tilt_frame = 'arm_pan_tilt_bracket'
self.arm_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.arm_tilt_pub = rospy.Publisher('arm_tilt_controller/command', Float64)
rospy.Subscriber('arm_tilt_controller/state', JointState, self.read_arm_tilt)
rospy.wait_for_message('arm_tilt_controller/state', JointState)
# Initialize publisher & subscriber for elbow tilt
self.elbow_tilt_frame = 'arm_bracket'
#self.elbow_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.elbow_tilt_pub = rospy.Publisher('elbow_tilt_controller/command', Float64)
rospy.Subscriber('elbow_tilt_controller/state', JointState, self.read_elbow_tilt)
rospy.wait_for_message('elbow_tilt_controller/state', JointState)
# Initialize publisher & subscriber for wrist pan
self.wrist_pan_frame = 'wrist_pan_link'
self.wrist_pan = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.wrist_pan_pub = rospy.Publisher('wrist_pan_controller/command', Float64)
rospy.Subscriber('wrist_pan_controller/state', JointState, self.read_wrist_pan)
rospy.wait_for_message('wrist_pan_controller/state', JointState)
# Initialize publisher & subscriber for wrist tilt
self.wrist_tilt_frame = 'wrist_tilt_link'
self.wrist_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.wrist_tilt_pub = rospy.Publisher('wrist_tilt_controller/command', Float64)
rospy.Subscriber('wrist_tilt_controller/state', JointState, self.read_wrist_tilt)
rospy.wait_for_message('wrist_tilt_controller/state', JointState)
# Initialize tf listener
self.tf = tf.TransformListener()
# Initialize joints action server
self.result = RobbieArmResult()
self.feedback = RobbieArmFeedback()
self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
self.server = SimpleActionServer(NAME, RobbieArmAction, self.execute_callback)
# Reset arm position
rospy.sleep(1)
self.reset_arm_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
def reset_arm_position(self):
# reset arm to cobra position
self.shoulder_pan_pub.publish(0.0)
self.arm_tilt_pub.publish(1.572222)
self.elbow_tilt_pub.publish(-1.572222)
self.wrist_pan_pub.publish(0.0)
self.wrist_tilt_pub.publish(0.0)
rospy.sleep(12)
def read_shoulder_pan(self, pan_data):
self.shoulder_pan = pan_data
self.has_latest_shoulder_pan = True
def read_arm_tilt(self, tilt_data):
self.arm_tilt = tilt_data
self.has_latest_arm_tilt = True
def read_elbow_tilt(self, tilt_data):
self.elbow_tilt = tilt_data
self.has_latest_elbow_tilt = True
def read_wrist_pan(self, rotate_data):
self.wrist_pan = rotate_data
self.has_latest_wrist_pan = True
def read_wrist_tilt(self, rotate_data):
self.wrist_tilt = rotate_data
self.has_latest_wrist_tilt = True
def wait_for_latest_controller_states(self, timeout):
self.has_latest_shoulder_pan = False
self.has_latest_arm_tilt = False
self.has_latest_elbow_tilt = False
self.has_latest_wrist_pan = False
self.has_latest_wrist_tilt = False
r = rospy.Rate(100)
start = rospy.Time.now()
while (self.has_latest_shoulder_pan == False or self.has_latest_arm_tilt == False or \
self.has_latest_elbow_tilt == False or self.has_latest_wrist_tilt == False or self.has_latest_wrist_pan == False) and \
(rospy.Time.now() - start < timeout):
r.sleep()
def transform_target_point(self, point):
rospy.loginfo("%s: Retrieving IK solutions", NAME)
rospy.wait_for_service('smart_arm_ik_service', 10)
ik_service = rospy.ServiceProxy('smart_arm_ik_service', SmartArmIK)
resp = ik_service(point)
if (resp and resp.success):
return resp.solutions[0:4]
else:
raise Exception, "Unable to obtain IK solutions."
def execute_callback(self, goal):
r = rospy.Rate(100)
self.result.success = True
self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
rospy.loginfo("%s: Executing move arm", NAME)
# Initialize target joints
target_joints = list()
for i in range(self.JOINTS_COUNT):
target_joints.append(0.0)
# Retrieve target joints from goal
if (len(goal.target_joints) > 0):
for i in range(min(len(goal.target_joints), len(target_joints))):
target_joints[i] = goal.target_joints[i]
else:
try:
# Convert target point to target joints (find an IK solution)
target_joints = self.transform_target_point(goal.target_point)
except (Exception, tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("%s: Aborted: IK Transform Failure", NAME)
self.result.success = False
self.server.set_aborted()
return
# Publish goal to controllers
self.shoulder_pan_pub.publish(target_joints[0])
self.arm_tilt_pub.publish(target_joints[1])
self.elbow_tilt_pub.publish(target_joints[2])
self.wrist_pan_pub.publish(target_joints[3])
self.wrist_tilt_pub.publish(target_joints[4])
# Initialize loop variables
start_time = rospy.Time.now()
while (math.fabs(target_joints[0] - self.shoulder_pan.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[1] - self.arm_tilt.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[2] - self.elbow_tilt.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[3] - self.wrist_pan.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[4] - self.wrist_tilt.process_value) > self.ERROR_THRESHOLD):
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
break
# Publish current arm position as feedback
self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > self.TIMEOUT_THRESHOLD):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
break
r.sleep()
if (self.result.success):
rospy.loginfo("%s: Goal Completed", NAME)
self.wait_for_latest_controller_states(rospy.Duration(2.0))
self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
self.server.set_succeeded(self.result)
class GotoServer:
def __init__(self):
self.rate = rospy.Rate(10)
self.goto_server = SimpleActionServer('goto',
GotoAction,
execute_cb=self.goto_execute,
auto_start=False)
self.goto_server.start()
self.last_object = "base1"
def goto_execute(self, goal):
rospy.loginfo("Executing {} for {}".format(
Goal.types[goal.type], Task.types[goal.task_number]))
rospy.loginfo("error_step={} error_object={}".format(
goal.error_step, goal.error_object))
outcome = "error"
if goal.type == Goal.BASE:
self.goto_feedback(Status.STARTED, "GO TO BASE SMACH STARTED")
rospy.loginfo("Initiating GotoBase State Machine")
sm = GotoObjectSMACH(last_object=self.last_object)
outcome = sm.execute(task_number=goal.task_number,
error_step=goal.error_step,
base=True)
elif goal.type == Goal.HUMAN:
self.goto_feedback(Status.STARTED, "FIND PERSON SMACH STARTED")
rospy.loginfo("Initiating FindPerson State Machine")
sm = FindPersonSMACH(last_object=self.last_object)
outcome = sm.execute(task_number=goal.task_number,
error_step=goal.error_step)
elif goal.type == Goal.OBJECT:
self.goto_feedback(Status.STARTED, "GO TO OBJECT SMACH STARTED")
rospy.loginfo("Initiating GotoObject State Machine")
sm = GotoObjectSMACH(last_object=self.last_object)
outcome = sm.execute(task_number=goal.task_number,
error_step=goal.error_step,
base=False)
self.last_object = sm.last_object
is_success = True if outcome == "finish" else False
if is_success:
self.goto_feedback(Status.COMPLETED, "SMACH SUCCESSFUL")
rospy.loginfo("State machine successful")
else:
self.goto_feedback(Status.FAILED, "SMACH FAILED")
rospy.logwarn("State machine failed")
goto_result = GotoResult()
goto_result.status = Status.COMPLETED if is_success else Status.FAILED
goto_result.is_complete = is_success
self.goto_server.set_succeeded(goto_result)
def goto_feedback(self, status, text):
feedback = GotoFeedback()
feedback.status = status
feedback.text = text
self.goto_server.publish_feedback(feedback)
class PathExecutor:
_continue=True
_skip_unreachable=True
_path_result=ExecutePathResult()
def __init__(self):
self._poses = []
self._poses_idx = 0
self._client = SimpleActionClient('/bug2/move_to', MoveToAction)
self._action_name = rospy.get_name() + "/execute_path"
self._server = SimpleActionServer(self._action_name, ExecutePathAction, execute_cb=self.execute_cb, auto_start = False)
self._server.start()
def execute_cb(self, goal):
"""
This funciton is used to execute the move to goal
"""
self._path_result.visited=[] #Initialize the path result
self._skip_unreachable=goal.skip_unreachable
self._client.wait_for_server()
self._poses = goal.path.poses
self._poses_idx = 0 #Start with the first goal
self._continue= True
rospy.loginfo('Starting Number of Poses: %s'%len(self._poses))
move = MoveToGoal()
move.target_pose.pose = self._poses[self._poses_idx].pose
"""
Send the goal to the _client and once done, go to callback function
"move_to_done_cb"
"""
self._client.send_goal(move, done_cb=self.move_to_done_cb)
while (self._continue==True):
"""
Check if current goal is preempted by other user action.
If so, break out of loop
"""
if self._server.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._server.set_preempted()
self._continue= False
break
def move_to_done_cb(self, state, result):
"""
This call back function is called once one goal is completed.
This checks if there are any other goal remaining to be executed and
recurrsively calls itself.
"""
feedback = ExecutePathFeedback()
feedback.pose=self._poses[self._poses_idx]
"""
#======================================================================
#* If state is 3, i.e., goal was successful, publish feedback
# and move to next goal
#
#* Else publish feedback and check _skip_unreachable flag,
# * If True go to next pose
# * Else break
#======================================================================
"""
if(state == 3):
feedback.reached = True
self._path_result.visited.append(feedback.reached)
self._server.publish_feedback(feedback)
self._poses_idx = self._poses_idx + 1
"""
* If more pose available, move to next goal
* Else break
"""
if(self._poses_idx < len(self._poses)):
move = MoveToGoal()
move.target_pose.pose = self._poses[self._poses_idx].pose
self._client.send_goal(move, done_cb=self.move_to_done_cb)
else:
self._continue=False
self._server.set_succeeded(self._path_result)
else:
feedback.reached = False
self._path_result.visited.append(False)
self._server.publish_feedback(feedback)
if(self._skip_unreachable==True):
self._poses_idx = self._poses_idx + 1
"""
* If more pose available, move to next goal
* Else break
"""
if(self._poses_idx < len(self._poses)):
move = MoveToGoal()
move.target_pose.pose = self._poses[self._poses_idx].pose
self._client.send_goal(move, done_cb=self.move_to_done_cb)
else:
self._continue=False
self._server.set_succeeded(self._path_result)
else:
rospy.loginfo('Unreachable')
self._continue=False
self._server.set_succeeded(self._path_result)
class AbstractHMIServer(object):
"""
Abstract base class for a hmi servers
"""
__metaclass__ = ABCMeta
def __init__(self, name):
self._server = SimpleActionServer(name,
QueryAction,
execute_cb=self._execute_cb,
auto_start=False)
self._server.start()
rospy.loginfo('HMI server started on "%s"', name)
def _execute_cb(self, goal):
rospy.loginfo('I got a question: %s', goal.description)
rospy.loginfo('This is the grammar: %s, %s', trim_string(goal.grammar),
goal.target)
try:
result = self._determine_answer(
description=goal.description,
example_sentences=goal.example_sentences,
grammar=goal.grammar,
target=goal.target,
is_preempt_requested=self._is_preempt_requested)
except Exception as e:
tb = traceback.format_exc()
rospy.logerr('_determine_answer raised an exception: %s' % tb)
self._set_aborted()
else:
# we've got a result or a cancel
if result:
rospy.loginfo('result: %s', result)
self._set_succeeded(result=result_to_ros(result))
else:
msg = "Cancelled by user"
rospy.loginfo(msg)
self._set_aborted(text=msg)
def _set_succeeded(self, result):
self._server.set_succeeded(result)
def _set_aborted(self, text=""):
self._server.set_aborted(text=text)
def _publish_feedback(self):
self._server.publish_feedback(QueryActionFeedback())
def _is_preempt_requested(self):
return self._server.is_preempt_requested()
@abstractmethod
def _determine_answer(self, description, grammar, target,
is_preempt_requested):
"""
Overwrite this method to provide custom implementations
Return the answer
Return None if nothing is heared
Raise an Exception if an error occured
"""
pass
class MoveBaseServer(MockActionServer):
def __init__(self, name, topic):
self._name = name
self.action_result = None
self._feedback = move_base_msgs.msg.MoveBaseFeedback()
self._result = move_base_msgs.msg.MoveBaseResult()
self.abort_with_feedback = False
self._topic = topic
Subscriber('mock/feedback_' + name, String, self.receive_commands)
self._server = ActionServer(self._topic, MoveBaseAction, self.execute,
False)
self._server.start()
loginfo('>>> Starting ' + self._name + ' with feedback.')
def receive_commands(self, msg):
callback, timeout = msg.data.split(':')
self.timeout = float(timeout)
if callback == 'abort_feedback':
self.abort_with_feedback = True
self._server.execute_callback = getattr(self, 'execute')
else:
self.abort_with_feedback = False
self._server.execute_callback = getattr(self, callback)
logwarn('>>> ' + self._name + ': Current callback -> ' + callback)
sleep(1)
def execute(self, goal):
success = True
start_x = 0
start_y = 0
goal_x = goal.target_pose.pose.position.x
goal_y = goal.target_pose.pose.position.y
loginfo('Goal -> x(%d), y(%d).', goal_x, goal_y)
if self.abort_with_feedback:
sleep(self.timeout)
self._feedback.base_position.pose.position.x = goal_x + 0.05
self._feedback.base_position.pose.position.y = goal_y + 0.05
self._server.publish_feedback(self._feedback)
sleep(self.timeout)
self._server.set_aborted()
return
while True:
# Make a move.
if start_x < goal_x:
start_x += 0.5
if start_y < goal_y:
start_y += 0.5
# Send feedback
self._feedback.base_position.pose.position.x = start_x
self._feedback.base_position.pose.position.y = start_y
self._server.publish_feedback(self._feedback)
if start_x >= goal_x and start_y >= goal_y:
break
sleep(self.timeout)
if success:
loginfo('MoveBaseAction: Goal succeeded...')
_result = self._feedback
self._server.set_succeeded(_result)
class HokuyoLaserActionServer():
def __init__(self):
# Initialize constants
self.error_threshold = 0.0175 # Report success if error reaches below threshold
self.signal = 1
# Initialize new node
rospy.init_node(NAME, anonymous=True)
controller_name = rospy.get_param('~controller')
# Initialize publisher & subscriber for tilt
self.laser_tilt = JointControllerState()
self.laser_tilt_pub = rospy.Publisher(controller_name + '/command',
Float64)
self.laser_signal_pub = rospy.Publisher('laser_scanner_signal',
LaserScannerSignal)
self.joint_speed_srv = rospy.ServiceProxy(controller_name +
'/set_speed',
SetSpeed,
persistent=True)
rospy.Subscriber(controller_name + '/state', JointControllerState,
self.process_tilt_state)
rospy.wait_for_message(controller_name + '/state',
JointControllerState)
# Initialize tilt action server
self.result = HokuyoLaserTiltResult()
self.feedback = HokuyoLaserTiltFeedback()
self.feedback.tilt_position = self.laser_tilt.process_value
self.server = SimpleActionServer('hokuyo_laser_tilt_action',
HokuyoLaserTiltAction,
self.execute_callback)
rospy.loginfo("%s: Ready to accept goals", NAME)
def process_tilt_state(self, tilt_data):
self.laser_tilt = tilt_data
def reset_tilt_position(self, offset=0.0):
self.laser_tilt_pub.publish(offset)
rospy.sleep(0.5)
def execute_callback(self, goal):
r = rospy.Rate(100)
self.joint_speed_srv(2.0)
self.reset_tilt_position(goal.offset)
delta = goal.amplitude - goal.offset
target_speed = delta / goal.duration
timeout_threshold = rospy.Duration(5.0) + rospy.Duration.from_sec(
goal.duration)
self.joint_speed_srv(target_speed)
print "delta = %f, target_speed = %f" % (delta, target_speed)
self.result.success = True
self.result.tilt_position = self.laser_tilt.process_value
rospy.loginfo("%s: Executing laser tilt %s time(s)", NAME,
goal.tilt_cycles)
# Tilt laser goal.tilt_cycles amount of times.
for i in range(1, goal.tilt_cycles + 1):
self.feedback.progress = i
# Issue 2 commands for each cycle
for j in range(2):
if j % 2 == 0:
target_tilt = goal.offset + goal.amplitude # Upper tilt limit
self.signal = 0
else:
target_tilt = goal.offset # Lower tilt limit
self.signal = 1
# Publish target command to controller
self.laser_tilt_pub.publish(target_tilt)
start_time = rospy.Time.now()
current_time = start_time
while abs(target_tilt - self.laser_tilt.process_value
) > self.error_threshold:
#delta = abs(target_tilt - self.laser_tilt.process_value)
#time_left = goal.duration - (rospy.Time.now() - start_time).to_sec()
#target_speed = delta / time_left
#self.joint_speed_srv(target_speed)
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
return
# Publish current head position as feedback
self.feedback.tilt_position = self.laser_tilt.process_value
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > timeout_threshold):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
return
r.sleep()
signal = LaserScannerSignal()
signal.header.stamp = current_time
signal.signal = self.signal
self.laser_signal_pub.publish(signal)
#rospy.sleep(0.5)
if self.result.success:
rospy.loginfo("%s: Goal Completed", NAME)
self.result.tilt_position = self.laser_tilt.process_value
self.server.set_succeeded(self.result)
class Explore(object):
def __init__(self, height):
self.motion_height = height # Height of the motion to the ground
# Create trajectory server
self.exploration_server = SimpleActionServer('assessment_server',
ExecuteAssesstAction,
self.exploreCallback,
False)
self.server_feedback = ExecuteAssesstFeedback()
self.server_result = ExecuteAssesstResult()
# Get client from trajectory server
self.trajectory_client = SimpleActionClient(
"approach_server", ExecuteDroneApproachAction)
self.trajectory_client.wait_for_server()
self.next_point = ExecuteDroneApproachGoal(
) # Message to define next position to look for victims
#Planning scene client
self.frontiers_client = rospy.ServiceProxy('frontiers_server/find',
Frontiers)
self.frontiers_client.wait_for_service()
self.frontiers_req = FrontiersRequest() #Frontiers request message
# Variables
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
self.odometry = None
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("ground_truth/state", Odometry, self.poseCallback)
self.scene_req = GetPlanningSceneRequest()
# Start trajectory server
self.exploration_server.start()
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
self.sonar_me.release()
def poseCallback(self, odometry):
'''
Monitor the current position of the robot
'''
self.odometry_me.acquire()
self.odometry = odometry.pose.pose
self.odometry_me.notify()
self.odometry_me.release()
def trajectory_feed(self, msg):
'''
Verifies preemption requisitions
'''
print("\n\n\nASSESSMENT FEEDBACK")
if self.exploration_server.is_preempt_requested():
self.trajectory_client.cancel_goal()
def exploreCallback(self, pose):
'''
Execute a loop looking for frontiers and moving to points unvisited into the defined area
'''
# Wait till the robot pose is received
self.odometry_me.acquire()
while self.odometry == None:
self.odometry_me.wait()
self.next_point.goal = self.odometry
self.odometry_me.release()
self.frontiers_req.x_min = 0.0
self.frontiers_req.x_max = 50.0
self.frontiers_req.y_min = 0.0
self.frontiers_req.y_max = 50.0
trials = 0 # v_search trials
while not rospy.is_shutdown():
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.server_feedback.current_pose = self.odometry
self.odometry_me.release()
if self.exploration_server.is_preempt_requested():
self.exploration_server.set_preempted(self.server_result)
return
self.exploration_server.publish_feedback(self.server_feedback)
self.sonar_me.acquire()
# print("Current height from ground:\n\n{}".format(self.current_height)) # Current distance from ground
h_error = self.motion_height - self.current_height
self.sonar_me.release()
self.odometry_me.acquire()
self.next_point.goal.position.z = self.odometry.position.z + h_error # Desired z position
self.odometry_me.release()
self.trajectory_client.send_goal(self.next_point,
feedback_cb=self.trajectory_feed)
self.trajectory_client.wait_for_result() # Wait for the result
result = self.trajectory_client.get_state(
) # Get the state of the action
# print(result)
if result == GoalStatus.SUCCEEDED:
p = Pose()
self.odometry_me.acquire()
p = self.odometry
self.frontiers_req.explored.append(p)
self.odometry_me.release()
# Verify if all the area have been explored and find next frontier point if needed
# if 'all area explored':
# self.odometry_me.acquire()
# self.server_result.last_pose = self.odometry
# self.odometry_me.release()
# self.exploration_server.set_succeeded(self.server_result)
status = self.findFrontiers()
if not status:
self.exploration_server.set_succeeded(self.server_result)
return
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.odometry_me.release()
# self.next_point.goal.position.y =
# self.next_point.goal.position.x =
# theta =
# Convert desired angle
# q = quaternion_from_euler(0,0,theta,'ryxz')
# self.next_point.goal.orientation.x = q[0]
# self.next_point.goal.orientation.y = q[1]
# self.next_point.goal.orientation.z = q[2]
# self.next_point.goal.orientation.w = q[3]
elif result == GoalStatus.ABORTED:
trials += 1
if trials == 2:
self.exploration_server.set_aborted(self.server_result)
return
def findFrontiers(self):
'''
Return points not visited into the specified frontier
'''
rospy.loginfo("Looking for frontiers!")
frontiers = self.frontiers_client.call(self.frontiers_req)
if frontiers.frontiers:
self.next_point.goal.position.x = frontiers.frontiers[0].x
self.next_point.goal.position.y = frontiers.frontiers[0].y
self.next_point.goal.position.x = self.motion_height
return True
else:
return False
class HokuyoLaserActionServer():
def __init__(self):
# Initialize constants
self.error_threshold = 0.0175 # Report success if error reaches below threshold
self.signal = 1
# Initialize new node
rospy.init_node(NAME, anonymous=True)
controller_name = rospy.get_param('~controller')
# Initialize publisher & subscriber for tilt
self.laser_tilt = JointControllerState()
self.laser_tilt_pub = rospy.Publisher(controller_name + '/command', Float64)
self.laser_signal_pub = rospy.Publisher('laser_scanner_signal', LaserScannerSignal)
self.joint_speed_srv = rospy.ServiceProxy(controller_name + '/set_speed', SetSpeed, persistent=True)
rospy.Subscriber(controller_name + '/state', JointControllerState, self.process_tilt_state)
rospy.wait_for_message(controller_name + '/state', JointControllerState)
# Initialize tilt action server
self.result = HokuyoLaserTiltResult()
self.feedback = HokuyoLaserTiltFeedback()
self.feedback.tilt_position = self.laser_tilt.process_value
self.server = SimpleActionServer('hokuyo_laser_tilt_action', HokuyoLaserTiltAction, self.execute_callback)
rospy.loginfo("%s: Ready to accept goals", NAME)
def process_tilt_state(self, tilt_data):
self.laser_tilt = tilt_data
def reset_tilt_position(self, offset=0.0):
self.laser_tilt_pub.publish(offset)
rospy.sleep(0.5)
def execute_callback(self, goal):
r = rospy.Rate(100)
self.joint_speed_srv(2.0)
self.reset_tilt_position(goal.offset)
delta = goal.amplitude - goal.offset
target_speed = delta / goal.duration
timeout_threshold = rospy.Duration(5.0) + rospy.Duration.from_sec(goal.duration)
self.joint_speed_srv(target_speed)
print "delta = %f, target_speed = %f" % (delta, target_speed)
self.result.success = True
self.result.tilt_position = self.laser_tilt.process_value
rospy.loginfo("%s: Executing laser tilt %s time(s)", NAME, goal.tilt_cycles)
# Tilt laser goal.tilt_cycles amount of times.
for i in range(1, goal.tilt_cycles + 1):
self.feedback.progress = i
# Issue 2 commands for each cycle
for j in range(2):
if j % 2 == 0:
target_tilt = goal.offset + goal.amplitude # Upper tilt limit
self.signal = 0
else:
target_tilt = goal.offset # Lower tilt limit
self.signal = 1
# Publish target command to controller
self.laser_tilt_pub.publish(target_tilt)
start_time = rospy.Time.now()
current_time = start_time
while abs(target_tilt - self.laser_tilt.process_value) > self.error_threshold:
#delta = abs(target_tilt - self.laser_tilt.process_value)
#time_left = goal.duration - (rospy.Time.now() - start_time).to_sec()
#target_speed = delta / time_left
#self.joint_speed_srv(target_speed)
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
return
# Publish current head position as feedback
self.feedback.tilt_position = self.laser_tilt.process_value
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > timeout_threshold):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
return
r.sleep()
signal = LaserScannerSignal()
signal.header.stamp = current_time
signal.signal = self.signal
self.laser_signal_pub.publish(signal)
#rospy.sleep(0.5)
if self.result.success:
rospy.loginfo("%s: Goal Completed", NAME)
self.result.tilt_position = self.laser_tilt.process_value
self.server.set_succeeded(self.result)
class FakeGrasping:
ALWAYS = 0
NEVER = 1
RANDOM = 2
def __init__(self):
self.server = SimpleActionServer('/robot_move',
RobotMoveAction,
execute_cb=self.robot_move_cb)
self.objects = self.ALWAYS
self.grasp = self.ALWAYS
self.place = self.ALWAYS
self.object_randomness = 0.8 # 80% of time object is known
self.grasp_randomness = 0.4
self.place_randomness = 0.4
self.holding = None
self.pick_length = 2 # how long (sec) takes to pick an object
self.place_length = 2 # how long (sec) takes to place an object
self.robot_state_pub = rospy.Publisher("/robot_status",
RobotStatus,
queue_size=1,
latch=True)
self.publish_robot_status(RobotStatus.STOPPED)
random.seed()
def publish_robot_status(self, status):
self.robot_state_pub.publish(
RobotStatus(
status=status,
actual_pose=PoseStamped(header=Header(frame_id="marker"))))
def robot_move_cb(self, goal):
self.publish_robot_status(RobotStatus.MOVING)
self.robot_move(goal)
self.publish_robot_status(RobotStatus.STOPPED)
def robot_move(self, goal):
result = RobotMoveResult()
feedback = RobotMoveFeedback()
if not (goal.move_type in (goal.HOME, goal.PROGRAMING, goal.PICK,
goal.PLACE, goal.MOVE)):
result.result = RobotMoveResult.BAD_REQUEST
rospy.logerr("BAD_REQUEST, Unknown operation")
self.server.set_aborted(result, "Unknown operation")
return
if self.objects == self.ALWAYS:
pass
elif self.objects == self.NEVER:
result.result = RobotMoveResult.BAD_REQUEST
rospy.logerr("BAD_REQUEST, Unknown object id")
self.server.set_aborted(result, "Unknown object id")
return
elif self.objects == self.RANDOM:
nmbr = random.random()
if nmbr > self.object_randomness:
result.result = RobotMoveResult.BAD_REQUEST
rospy.logerr("BAD_REQUEST, Unknown object id")
self.server.set_aborted(result, "Unknown object id")
return
grasped = False
if goal.move_type == RobotMoveGoal.PICK:
rospy.sleep(self.pick_length)
if False and self.holding:
result.result = RobotMoveResult.BUSY
rospy.logerr("Failure, already holding object in arm")
self.server.set_aborted(result,
"Already holding object in arm")
return
if self.grasp == self.ALWAYS:
grasped = True
pass
elif self.grasp == self.NEVER:
result.result = RobotMoveResult.MOVE_FAILURE
rospy.logerr("FAILURE, Pick Failed")
self.server.set_aborted(result, "Pick Failed")
return
tries = 5
while tries > 0:
feedback.state = feedback.PICKING
tries -= 1
self.server.publish_feedback(feedback)
if self.grasp == self.RANDOM:
nmbr = random.random()
if nmbr < self.grasp_randomness:
grasped = True
if grasped:
break
if self.server.is_preempt_requested():
self.server.set_preempted(result, "Pick canceled")
rospy.logerr("Preempted")
return
if not grasped:
result.result = RobotMoveResult.FAILURE
self.server.set_aborted(result, "Pick failed")
rospy.logerr("FAILURE, Pick Failed")
return
else:
self.holding = True
placed = False
if goal.move_type == RobotMoveGoal.PLACE:
rospy.sleep(self.place_length)
if not self.holding:
result.result = RobotMoveResult.MOVE_FAILURE
rospy.logerr("Failure, robot not holding object")
self.server.set_aborted(result, "Robot not holding object")
return
if self.place == self.ALWAYS:
placed = True
pass
elif self.place == self.NEVER:
result.result = RobotMoveResult.MOVE_FAILURE
self.server.set_aborted(result, "Place Failed")
rospy.logerr("FAILURE, Place Failed")
return
tries = 5
while tries > 0:
feedback.state = feedback.PLACING
tries -= 1
self.server.publish_feedback(feedback)
if self.place == self.RANDOM:
nmbr = random.random()
if nmbr < self.place_randomness:
placed = True
if placed:
break
if not placed:
result.result = RobotMoveResult.MOVE_FAILURE
self.server.set_aborted(result, "Place failed")
rospy.logerr("FAILURE, Place Failed")
return
else:
self.holding = False
result.result = RobotMoveResult.SUCCES
self.server.set_succeeded(result)
rospy.loginfo("SUCCESS")
print("Finished")
class AbstractHMIServer(object):
"""
Abstract base class for a hmi client
>>> class HMIServer(AbstractHMIServer):
... def __init__(self):
... pass
... def _determine_answer(self, description, spec, choices):
... return QueryResult()
... def _set_succeeded(self, result):
... print result
>>> server = HMIServer()
>>> from hmi_msgs.msg import QueryGoal
>>> goal = QueryGoal(description='q', spec='spec', choices=[])
>>> server._execute_cb(goal)
raw_result: ''
results: []
>>> class HMIServer(AbstractHMIServer):
... def __init__(self):
... pass
>>> server = HMIServer()
Traceback (most recent call last):
...
TypeError: Can't instantiate abstract class HMIServer with abstract methods _determine_answer
"""
__metaclass__ = ABCMeta
def __init__(self, name):
self._action_name = name
self._server = SimpleActionServer(name, QueryAction,
execute_cb=self._execute_cb, auto_start=False)
self._server.start()
rospy.loginfo('HMI server started on "%s"', name)
def _execute_cb(self, goal):
# TODO: refactor this somewhere
choices = {}
for choice in goal.choices:
if choice.id in choices:
rospy.logwarn('duplicate key "%s" in answer', choice.id)
else:
choices[choice.id] = choice.values
rospy.loginfo('I got a question: %s', goal.description)
rospy.loginfo('This is the spec: %s, %s', trim_string(goal.spec), repr(choices))
try:
result = self._determine_answer(description=goal.description,
spec=goal.spec,
choices=choices,
is_preempt_requested=self._server.is_preempt_requested)
except Exception as e:
# rospy.logwarn('_determine_answer raised an exception: %s', e)
# import pdb; pdb.set_trace()
tb = traceback.format_exc()
rospy.logerr('_determine_answer raised an exception: %s' % tb)
self._server.set_aborted()
else:
# we've got a result or a cancel
if result:
self._set_succeeded(result=result.to_ros(self._action_name))
rospy.loginfo('result: %s', result)
else:
rospy.loginfo('cancelled')
self._server.set_aborted(text="Cancelled by user")
def _set_succeeded(self, result):
self._server.set_succeeded(result)
def _publish_feedback(self):
self._server.publish_feedback(QueryActionFeedback())
@abstractmethod
def _determine_answer(self, description, spec, choices, is_preempt_requested):
'''
Overwrite this method to provide custom implementations
Return the answer
Return None if nothing is heared
Raise an Exception if an error occured
'''
pass
class RobotPolicyExecutor():
def __init__(self, port, file_dir, file_name):
self.wait_for_result_dur = rospy.Duration(0.1)
self.top_nav_policy_exec = SimpleActionClient(
'topological_navigation/execute_policy_mode',
ExecutePolicyModeAction)
got_server = self.top_nav_policy_exec.wait_for_server(
rospy.Duration(1))
while not got_server:
rospy.loginfo(
"Waiting for topological navigation execute policy mode action server."
)
got_server = self.top_nav_policy_exec.wait_for_server(
rospy.Duration(1))
if rospy.is_shutdown():
return
self.policy_mode_pub = rospy.Publisher(
"mdp_plan_exec/current_policy_mode", NavRoute, queue_size=1)
self.current_waypoint_sub = rospy.Subscriber("current_node", String,
self.current_waypoint_cb)
self.closest_waypoint_sub = rospy.Subscriber("closest_node", String,
self.closest_waypoint_cb)
explicit_doors = rospy.get_param("mdp_plan_exec/explicit_doors", True)
forget_doors = rospy.get_param("mdp_plan_exec/forget_doors", True)
model_fatal_fails = rospy.get_param("mdp_plan_exec/model_fatal_fails",
True)
self.nav_before_action_exec = rospy.get_param(
"mdp_plan_exec/nav_before_action_exec", True
) #If True the robot will always navigate to a waypoint before trying to execute an action; if False robot can execute actions anywhere, if they are added without waypoint constraints.
self.mdp = TopMapMdp(explicit_doors=explicit_doors,
forget_doors=forget_doors,
model_fatal_fails=model_fatal_fails)
self.policy_utils = PolicyExecutionUtils(port, file_dir, file_name,
self.mdp)
self.action_executor = ActionExecutor()
self.cancelled = False
self.mdp_as = SimpleActionServer('mdp_plan_exec/execute_policy',
ExecutePolicyAction,
execute_cb=self.execute_policy_cb,
auto_start=False)
self.mdp_as.register_preempt_callback(self.preempt_policy_execution_cb)
self.mdp_as.start()
def current_waypoint_cb(self, msg):
self.current_waypoint = msg.data
def closest_waypoint_cb(self, msg):
self.closest_waypoint = msg.data
def execute_nav_policy(self, nav_policy_msg):
self.policy_mode_pub.publish(nav_policy_msg)
self.regenerated_nav_policy = False
self.top_nav_policy_exec.send_goal(
ExecutePolicyModeGoal(route=nav_policy_msg),
feedback_cb=self.top_nav_feedback_cb)
top_nav_running = True
while top_nav_running and not self.cancelled:
top_nav_running = not self.top_nav_policy_exec.wait_for_result(
self.wait_for_result_dur)
if not top_nav_running:
if self.regenerated_nav_policy:
nav_policy_msg = self.policy_utils.generate_current_nav_policy(
)
print(nav_policy_msg)
self.top_nav_policy_exec.send_goal(
ExecutePolicyModeGoal(route=nav_policy_msg),
feedback_cb=self.top_nav_feedback_cb)
top_nav_running = True
self.regenerated_nav_policy = False
else:
status = self.top_nav_policy_exec.get_state()
if self.cancelled:
status = GoalStatus.PREEMPTED
return status
def top_nav_feedback_cb(self, feedback):
executed_action = self.policy_mdp.get_current_action()
next_flat_state = None
if feedback.status == GoalStatus.SUCCEEDED:
next_flat_state = self.policy_utils.get_next_nav_policy_state(
feedback.current_wp, self.policy_mdp)
if next_flat_state is None:
rospy.logwarn(
"Error getting MDP next state: There is no transition modelling the state evolution. Looking for state in full state list..."
)
next_flat_state = self.policy_utils.get_next_state_from_wp_update(
self.policy_mdp, feedback.current_wp)
self.top_nav_policy_exec.cancel_all_goals()
if next_flat_state is not None:
self.regenerated_nav_policy = True
publish = next_flat_state != self.policy_mdp.current_flat_state
self.policy_mdp.set_current_state(next_flat_state)
if publish:
next_action = self.policy_mdp.get_current_action()
self.publish_feedback(executed_action, feedback.status,
next_action)
def execute_policy_cb(self, goal):
self.cancelled = False
self.policy_mdp = self.policy_utils.generate_policy_mdp(
goal.spec, self.closest_waypoint, rospy.Time.now())
if self.policy_mdp is None:
rospy.logerr("Failure to build policy for specification: " +
goal.spec.ltl_task)
self.mdp_as.set_aborted()
return
self.publish_feedback(None, None, self.policy_mdp.get_current_action())
if self.nav_before_action_exec:
starting_exec = True #used to make sure the robot executes calls topological navigation at least once before executing non-nav actions. This is too ensure the robot navigates to the exact pose of a waypoint before executing an action there
else:
starting_exec = False
while (self.policy_mdp.has_action_defined()
and not self.cancelled) or starting_exec:
next_action = self.policy_mdp.get_current_action()
if next_action in self.mdp.nav_actions or starting_exec:
starting_exec = False
current_nav_policy = self.policy_utils.generate_current_nav_policy(
self.policy_mdp)
status = self.execute_nav_policy(current_nav_policy)
rospy.loginfo(
"Topological navigation execute policy action server exited with status: "
+ GoalStatus.to_string(status))
if status != GoalStatus.SUCCEEDED:
self.policy_mdp.set_current_state(None)
break
else:
print("EXECUTE ACTION")
(status, state_update) = self.action_executor.execute_action(
self.mdp.action_descriptions[next_action])
executed_action = next_action
print(executed_action)
if not self.cancelled:
next_flat_state = self.policy_utils.get_next_state_from_action_outcome(
state_update, self.policy_mdp)
self.policy_mdp.set_current_state(next_flat_state)
if next_flat_state is None:
rospy.logerr(
"Error finding next state after action execution. Aborting..."
)
break
next_action = self.policy_mdp.get_current_action()
self.publish_feedback(executed_action, status, next_action)
if self.cancelled:
self.cancelled = False
rospy.loginfo("Policy execution preempted.")
self.mdp_as.set_preempted()
elif self.policy_mdp.current_flat_state is None:
rospy.loginfo("Policy execution failed.")
self.mdp_as.set_aborted()
else:
rospy.loginfo("Policy execution successful.")
self.mdp_as.set_succeeded()
def publish_feedback(self, executed_action, status, next_action):
(probability, prog_reward,
expected_time) = self.policy_mdp.get_guarantees_at_current_state()
self.mdp_as.publish_feedback(
ExecutePolicyFeedback(probability=probability,
expected_time=expected_time,
prog_reward=prog_reward,
current_waypoint=self.current_waypoint,
executed_action=executed_action,
execution_status=status,
next_action=next_action))
def preempt_policy_execution_cb(self):
self.top_nav_policy_exec.cancel_all_goals()
self.action_executor.cancel_all_goals()
self.cancelled = True
def main(self):
# Wait for control-c
rospy.spin()
if rospy.is_shutdown():
self.policy_utils.shutdown_prism(True)
class PathExecutor:
_feedback = ExecutePathFeedback()
_result = ExecutePathResult()
def __init__(self, name):
# self._goal_publisher = Simple
# self.pe_client_for_bug2= SimpleActionClient('/bug2/move_to',MoveToAction)
# self.pe_client_for_bug2.wait_for_server()
print "----------------- in INIT ---------------------------------------"
rospy.loginfo("Creating an action server")
# Creating a client to perform the bug2 function
self._move_to_client = SimpleActionClient('/bug2/move_to',
MoveToAction)
# Creating an action server
self._as = SimpleActionServer('/path_executor/execute_path', ExecutePathAction, \
execute_cb=self.execute_cb, auto_start=False)
# Starting the server
self._as.start()
# Using a flag to aid preemption
self.flag = True
# Using a flag to detect the completion of 3 goals
self.success = True
def execute_cb(self, goal):
# Executed every time a goal is sent by the client
self._move_to_client.wait_for_server()
# printing the goals in the terminal
rospy.loginfo(goal.path.poses)
# Creating an instance of MoveToGoal to compose a goal message
self.move = MoveToGoal()
# Iterating through the goals and sending it to the move_to_client
for i in range(len(goal.path.poses)):
if i == 2:
self.success = True
else:
self.success = False
self._pose = goal.path.poses[i]
self.move.target_pose = self._pose
self._move_to_client.send_goal(self.move,
done_cb=self.move_to_done_cb)
self._move_to_client.wait_for_result()
# self.success = True
rospy.spin()
# setting the server's preempted() function if it's requested
while flag:
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
break
def move_to_done_cb(self, state, result):
if (state == 3):
#
self._result.visited.append(True)
self._feedback.reached = True
self._feedback.pose = self._pose
elif (state == 4):
self._feedback.reached = False
#
rospy.loginfo(self._result)
self._as.publish_feedback(self._feedback)
if self.success == True:
print "All goals are considered"
self._as.set_succeeded(self._result)
class RobbieHeadActionServer():
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 2 # Number of joints to manage
self.ERROR_THRESHOLD = 0.02 # Report success if error reaches below threshold (0.015 also works)
self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME, anonymous=True)
# Initialize publisher & subscriber for pan
self.head_pan_frame = 'head_pan_link'
self.head_pan = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.head_pan_pub = rospy.Publisher('head_pan_controller/command', Float64)
rospy.Subscriber('head_pan_controller/state_pr2_msgs', JointControllerState, self.read_current_pan)
rospy.wait_for_message('head_pan_controller/state_pr2_msgs', JointControllerState)
# Initialize publisher & subscriber for tilt
self.head_tilt_frame = 'head_tilt_link'
self.head_tilt = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.head_tilt_pub = rospy.Publisher('head_tilt_controller/command', Float64)
rospy.Subscriber('head_tilt_controller/state_pr2_msgs', JointControllerState, self.read_current_tilt)
rospy.wait_for_message('head_tilt_controller/state_pr2_msgs', JointControllerState)
# Initialize tf listener
self.tf = tf.TransformListener()
# Initialize point action server
self.result = RobbieHeadResult()
self.feedback = RobbieHeadFeedback()
self.feedback.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
self.server = SimpleActionServer(NAME, RobbieHeadAction, self.execute_callback, auto_start=False)
# Reset head position
rospy.sleep(1)
self.reset_head_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
def read_current_pan(self, pan_data):
self.head_pan = pan_data
self.has_latest_pan = True
def read_current_tilt(self, tilt_data):
self.head_tilt = tilt_data
self.has_latest_tilt = True
def reset_head_position(self):
self.head_pan_pub.publish(0.0)
self.head_tilt_pub.publish(0.0)
rospy.sleep(5)
def wait_for_latest_controller_states(self, timeout):
self.has_latest_pan = False
self.has_latest_tilt = False
r = rospy.Rate(100)
start = rospy.Time.now()
while (self.has_latest_pan == False or self.has_latest_tilt == False) and (rospy.Time.now() - start < timeout):
r.sleep()
def transform_target_point(self, point):
pan_target_frame = self.head_pan_frame
tilt_target_frame = self.head_tilt_frame
# Wait for tf info (time-out in 5 seconds)
self.tf.waitForTransform(pan_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
self.tf.waitForTransform(tilt_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
# Transform target point to pan reference frame & retrieve the pan angle
pan_target = self.tf.transformPoint(pan_target_frame, point)
pan_angle = math.atan2(pan_target.point.y, pan_target.point.x)
#rospy.loginfo("%s: Pan transformed to <%s, %s, %s> => angle %s", \
# NAME, pan_target.point.x, pan_target.point.y, pan_target.point.z, pan_angle)
# Transform target point to tilt reference frame & retrieve the tilt angle
tilt_target = self.tf.transformPoint(tilt_target_frame, point)
tilt_angle = math.atan2(tilt_target.point.z,
math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2)))
#rospy.loginfo("%s: Tilt transformed to <%s, %s, %s> => angle %s", \
# NAME, tilt_target.point.x, tilt_target.point.y, tilt_target.point.z, tilt_angle)
return [pan_angle, tilt_angle]
def execute_callback(self, goal):
r = rospy.Rate(100)
self.result.success = True
self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
rospy.loginfo("%s: Executing move head", NAME)
# Initialize target joints
target_joints = list()
for i in range(self.JOINTS_COUNT):
target_joints.append(0.0)
# Retrieve target joints from goal
if (len(goal.target_joints) > 0):
for i in range(min(len(goal.target_joints), len(target_joints))):
target_joints[i] = goal.target_joints[i]
else:
try:
# Try to convert target point to pan & tilt angles
target_joints = self.transform_target_point(goal.target_point)
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("%s: Aborted: Transform Failure", NAME)
self.result.success = False
self.server.set_aborted()
return
# Publish goal command to controllers
self.head_pan_pub.publish(target_joints[0])
self.head_tilt_pub.publish(target_joints[1])
# Initialize loop variables
start_time = rospy.Time.now()
while (math.fabs(target_joints[0] - self.head_pan.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[1] - self.head_tilt.process_value) > self.ERROR_THRESHOLD):
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
break
# Publish current head position as feedback
self.feedback.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > self.TIMEOUT_THRESHOLD):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
break
r.sleep()
if (self.result.success):
rospy.loginfo("%s: Goal Completed", NAME)
self.wait_for_latest_controller_states(rospy.Duration(2.0))
self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
self.server.set_succeeded(self.result)
class OnlineBagger(object):
BAG_TOPIC = '/online_bagger/bag'
def __init__(self):
"""
Make dictionary of dequeues.
Subscribe to set of topics defined by the yaml file in directory
Stream topics up to a given stream time, dump oldest messages when limit is reached
Set up service to bag n seconds of data default to all of available data
"""
self.successful_subscription_count = 0 # successful subscriptions
self.iteration_count = 0 # number of iterations
self.streaming = True
self.get_params()
if len(self.subscriber_list) == 0:
rospy.logwarn('No topics selected to subscribe to. Closing.')
rospy.signal_shutdown('No topics to subscribe to')
return
self.make_dicts()
self._action_server = SimpleActionServer(OnlineBagger.BAG_TOPIC,
BagOnlineAction,
execute_cb=self.start_bagging,
auto_start=False)
self.subscribe_loop()
rospy.loginfo('Remaining Failed Topics: {}\n'.format(
self.get_subscriber_list(False)))
self._action_server.start()
def get_subscriber_list(self, status):
"""
Get string of all topics, if their subscribe status matches the input (True / False)
Outputs each topics: time_buffer(float in seconds), subscribe_statue(bool), topic(string)
"""
sub_list = ''
for topic in self.subscriber_list.keys():
if self.subscriber_list[topic][1] == status:
sub_list = sub_list + \
'\n{:13}, {}'.format(self.subscriber_list[topic], topic)
return sub_list
def get_params(self):
"""
Retrieve parameters from param server.
"""
self.dir = rospy.get_param('~bag_package_path', default=None)
# Handle bag directory for MIL bag script
if self.dir is None and 'BAG_DIR' in os.environ:
self.dir = os.environ['BAG_DIR']
self.stream_time = rospy.get_param('~stream_time',
default=30) # seconds
self.resubscribe_period = rospy.get_param('~resubscribe_period',
default=3.0) # seconds
self.dated_folder = rospy.get_param('~dated_folder',
default=True) # bool
self.subscriber_list = {}
topics_param = rospy.get_param('~topics', default=[])
# Add topics from rosparam to subscribe list
for topic in topics_param:
time = topic[1] if len(topic) == 2 else self.stream_time
self.subscriber_list[topic[0]] = (time, False)
def add_unique_topic(topic):
if topic not in self.subscriber_list:
self.subscriber_list[topic] = (self.stream_time, False)
def add_env_var(var):
for topic in var.split():
add_unique_topic(topic)
# Add topics from MIL bag script environment variables
if 'BAG_ALWAYS' in os.environ:
add_env_var(os.environ['BAG_ALWAYS'])
for key in os.environ.keys():
if key[0:4] == 'bag_':
add_env_var(os.environ[key])
rospy.loginfo('Default stream_time: {} seconds'.format(
self.stream_time))
rospy.loginfo('Bag Directory: {}'.format(self.dir))
def make_dicts(self):
"""
Make dictionary with sliceable deques() that will be filled with messages and time stamps.
Subscriber list contains all of the topics, their stream time and their subscription status:
A True status for a given topic corresponds to a successful subscription
A False status indicates a failed subscription.
Stream time for an individual topic is specified in seconds.
For Example:
self.subscriber_list[0:1] = [['/odom', 300 ,False], ['/absodom', 300, True]]
Indicates that '/odom' has not been subscribed to, but '/absodom' has been subscribed to
self.topic_messages is a dictionary of deques containing a list of tuples.
Dictionary Keys contain topic names
Each value for each topic contains a deque
Each deque contains a list of tuples
Each tuple contains a message and its associated time stamp
For example:
'/odom' is a potential topic name
self.topic_message['/odom'] is a deque
self.topic_message['/odom'][0] is the oldest message available in the deque
and its time stamp if available. It is a tuple with each element: (time_stamp, msg)
self.topic_message['/odom'][0][0] is the time stamp for the oldest message
self.topic_message['/odom'][0][1] is the message associated with the oldest topic
"""
self.topic_messages = {}
class SliceableDeque(deque):
def __getitem__(self, index):
if isinstance(index, slice):
return type(self)(itertools.islice(self, index.start,
index.stop, index.step))
return deque.__getitem__(self, index)
for topic in self.subscriber_list:
self.topic_messages[topic] = SliceableDeque(deque())
rospy.loginfo('Initial subscriber_list: {}'.format(
self.get_subscriber_list(False)))
def subscribe_loop(self):
"""
Continue to subscribe until at least one topic is successful,
then break out of loop and be called in the callback function to prevent the function
from locking up.
"""
self.resubscriber = None
i = 0
# if self.successful_subscription_count == 0 and not
# rospy.is_shutdown():
while self.successful_subscription_count == 0 and not rospy.is_shutdown(
):
self.subscribe()
rospy.sleep(0.1)
i = i + 1
if i % 1000 == 0:
rospy.logdebug('still subscribing!')
rospy.loginfo(
"Subscribed to {} of {} topics, will try again every {} seconds".
format(self.successful_subscription_count,
len(self.subscriber_list), self.resubscribe_period))
self.resubscriber = rospy.Timer(
rospy.Duration(self.resubscribe_period), self.subscribe)
def subscribe(self, time_info=None):
"""
Subscribe to the topics defined in the yaml configuration file
Function checks subscription status True/False of each topic
if True: topic has already been sucessfully subscribed to
if False: topic still needs to be subscribed to and
subscriber will be run.
Each element in self.subscriber list is a list [topic, Bool]
where the Bool tells the current status of the subscriber (sucess/failure).
Return number of topics that failed subscription
"""
if self.successful_subscription_count == len(self.subscriber_list):
if self.resubscriber is not None:
self.resubscriber.shutdown()
rospy.loginfo(
'All topics subscribed too! Shutting down resubscriber')
for topic, (time, subscribed) in self.subscriber_list.items():
if not subscribed:
msg_class = rostopic.get_topic_class(topic)
if msg_class[1] is not None:
self.successful_subscription_count += 1
rospy.Subscriber(topic,
msg_class[0],
lambda msg, _topic=topic: self.
bagger_callback(msg, _topic))
self.subscriber_list[topic] = (time, True)
def get_topic_duration(self, topic):
"""
Return current time duration of topic
"""
return self.topic_messages[topic][-1][0] - self.topic_messages[topic][
0][0]
def get_header_time(self, msg):
"""
Retrieve header time if available
"""
if hasattr(msg, 'header'):
return msg.header.stamp
else:
return rospy.get_rostime()
def get_time_index(self, topic, requested_seconds):
"""
Return the index for the time index for a topic at 'n' seconds from the end of the dequeue
For example, to bag the last 10 seconds of data, the index for 10 seconds back from the most
recent message can be obtained with this function.
The number of requested seconds should be the number of seoncds desired from
the end of deque. (ie. requested_seconds = 10 )
If the desired time length of the bag is greater than the available messages it will output a
message and return how ever many seconds of data are avaiable at the moment.
Seconds is of a number type (not a rospy.Time type) (ie. int, float)
"""
topic_duration = self.get_topic_duration(topic).to_sec()
if topic_duration == 0:
return 0
ratio = requested_seconds / topic_duration
index = int(self.get_topic_message_count(topic) * (1 - min(ratio, 1)))
return index
def bagger_callback(self, msg, topic):
"""
Streaming callback function, stops streaming during bagging process
also pops off msgs from dequeue if stream size is greater than specified stream_time
Stream, callback function does nothing if streaming is not active
"""
if not self.streaming:
return
self.iteration_count = self.iteration_count + 1
time = self.get_header_time(msg)
self.topic_messages[topic].append((time, msg))
time_diff = self.get_topic_duration(topic)
# verify streaming is popping off and recording topics
if self.iteration_count % 100 == 0:
rospy.logdebug("{} has {} messages spanning {} seconds".format(
topic, self.get_topic_message_count(topic),
round(time_diff.to_sec(), 2)))
while time_diff > rospy.Duration(
self.subscriber_list[topic][0]) and not rospy.is_shutdown():
self.topic_messages[topic].popleft()
time_diff = self.get_topic_duration(topic)
def get_topic_message_count(self, topic):
"""
Return number of messages available in a topic
"""
return len(self.topic_messages[topic])
def get_total_message_count(self):
"""
Returns total number of messages across all topics
"""
total_message_count = 0
for topic in self.topic_messages.keys():
total_message_count = total_message_count + \
self.get_topic_message_count(topic)
return total_message_count
def _get_default_filename(self):
return str(datetime.date.today()) + '-' + str(
datetime.datetime.now().time())[0:8]
def get_bag_name(self, filename=''):
"""
Create ros bag save directory
If no bag name is provided, the current date/time is used as default.
"""
# If directory param is not set, default to $HOME/bags/<date>
default_dir = self.dir
if default_dir is None:
default_dir = os.path.join(os.environ['HOME'], 'bags')
# if dated folder param is set to True, append current date to
# directory
if self.dated_folder is True:
default_dir = os.path.join(default_dir, str(datetime.date.today()))
# Split filename from directory
bag_dir, bag_name = os.path.split(filename)
bag_dir = os.path.join(default_dir, bag_dir)
if not os.path.exists(bag_dir):
os.makedirs(bag_dir)
# Create default filename if only directory specified
if bag_name == '':
bag_name = self._get_default_filename()
# Make sure filename ends in .bag, add otherwise
if bag_name[-4:] != '.bag':
bag_name = bag_name + '.bag'
return os.path.join(bag_dir, bag_name)
def start_bagging(self, req):
"""
Dump all data in dictionary to bags, temporarily stops streaming
during the bagging process, resumes streaming when over.
If bagging is already false because of an active call to this service
"""
result = BagOnlineResult()
if self.streaming is False:
result.status = 'Bag Request came in while bagging, priority given to prior request'
result.success = False
self._action_server.set_aborted(result)
return
try:
self.streaming = False
result.filename = self.get_bag_name(req.bag_name)
bag = rosbag.Bag(result.filename, 'w')
requested_seconds = req.bag_time
selected_topics = req.topics.split()
feedback = BagOnlineFeedback()
total_messages = 0
bag_topics = {}
for topic, (time, subscribed) in self.subscriber_list.iteritems():
if not subscribed:
continue
# Exclude topics that aren't in topics service argument
# If topics argument is empty string, include all topics
if len(selected_topics) > 0 and topic not in selected_topics:
continue
if len(self.topic_messages[topic]) == 0:
continue
if req.bag_time == 0:
index = 0
else:
index = self.get_time_index(topic, requested_seconds)
total_messages += len(self.topic_messages[topic][index:])
bag_topics[topic] = index
if total_messages == 0:
result.success = False
result.status = 'no messages'
self._action_server.set_aborted(result)
self.streaming = True
bag.close()
return
self._action_server.publish_feedback(feedback)
msg_inc = 0
for topic, index in bag_topics.iteritems():
for msgs in self.topic_messages[topic][index:]:
bag.write(topic, msgs[1], t=msgs[0])
if msg_inc % 50 == 0: # send feedback every 50 messages
feedback.progress = float(msg_inc) / total_messages
self._action_server.publish_feedback(feedback)
msg_inc += 1
# empty deque when done writing to bag
self.topic_messages[topic].clear()
feedback.progress = 1.0
self._action_server.publish_feedback(feedback)
bag.close()
except Exception as e:
result.success = False
result.status = 'Exception while writing bag: ' + str(e)
self._action_server.set_aborted(result)
self.streaming = True
bag.close()
return
rospy.loginfo('Bag written to {}'.format(result.filename))
result.success = True
self._action_server.set_succeeded(result)
self.streaming = True
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 SimpleMoveAction():
"""docstring for SimpleMoveAction"""
def __init__(self, name):
self.linear_speed = rospy.get_param('~linear_speed',
0.2) # measure from robot center
self.angular_speed = rospy.get_param('~angular_speed', 0.5)
self.linear_tolerance = 0.05 # theta
self.angular_tolerance = 10 # theta
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
self._action_name = name
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_footprint', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
self._feedback = sevenbot_navigation.msg.SimpleMoveFeedback()
self._result = sevenbot_navigation.msg.SimpleMoveResult()
self._as = SimpleActionServer(self._action_name, sevenbot_navigation.msg.SimpleMoveAction, \
execute_cb=self.execute_cb, auto_start = False)
self._as.start()
def get_odom(self):
# Get the current transform between the odom and base frames
try:
trans, quat = self.tf_listener.lookupTransform(
self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return trans, quat
def go_straight(self, dist_vec):
cmd_vel = Twist()
cmd_vel.linear.x = self.linear_speed
# Publish the Twist message and sleep 1 cycle
self.cmd_vel_pub.publish(cmd_vel)
# Post-update
trans, quat = self.get_odom()
dist_vec.x = self.goal.pose.position.x - trans[0]
dist_vec.y = self.goal.pose.position.y - trans[1]
r = rospy.Rate(20)
r.sleep()
return dist_vec
def turn_in_place(self, dist_vec):
cmd_vel = Twist()
cmd_vel.angular.z = self.angular_speed
# Publish the Twist message and sleep 1 cycle
self.cmd_vel_pub.publish(cmd_vel)
trans, quat = self.get_odom()
quat0 = [
self.goal.pose.orientation.w, self.goal.pose.orientation.x,
self.goal.pose.orientation.y, self.goal.pose.orientation.z
]
dist_vec.z = tftr.euler_from_quaternion(
tftr.quaternion_slerp(quat0, quat,
fraction=1))[2] # take only the Yaw value
r = rospy.Rate(20)
r.sleep()
return dist_vec
def execute_cb(self, goal):
self.goal = goal
self._result.reached = False
# How fast will we update the robot's movement?
dist_vec = Vector3()
trans, quat = self.get_odom()
dist_vec.x = self.goal.pose.position.x - trans[0]
dist_vec.y = self.goal.pose.position.y - trans[1]
# the goal contains a quat already turned, so this with end up with the error between current and goal
# dist_vec.z will be the euler angle theta of the error to goal
quat0 = [
self.goal.pose.orientation.w, self.goal.pose.orientation.x,
self.goal.pose.orientation.y, self.goal.pose.orientation.z
]
dist_vec.z = tftr.euler_from_quaternion(
tftr.quaternion_slerp(quat0, quat, fraction=1))[2]
print tftr.euler_from_quaternion(quat0)
print tftr.euler_from_quaternion(
tftr.quaternion_slerp(quat0, quat, fraction=1))
print('goal z', dist_vec.z)
distance = sqrt(dist_vec.x**2 + dist_vec.y**2)
while (distance -
self.linear_tolerance) > 0.0 and not rospy.is_shutdown():
dist_vec = self.go_straight(dist_vec)
distance = sqrt(dist_vec.x**2 + dist_vec.y**2)
self._feedback.vector = dist_vec
self._as.publish_feedback(self._feedback)
# rospy.loginfo('%s: Moving, distance to goal %f ' % (self._action_name, distance))
# Stop the robot before the rotation
cmd_vel = Twist()
self.cmd_vel_pub.publish(cmd_vel)
rospy.sleep(3)
# Now start turning
trans, quat = self.get_odom()
quat0 = [
self.goal.pose.orientation.w, self.goal.pose.orientation.x,
self.goal.pose.orientation.y, self.goal.pose.orientation.z
]
# update the orientation again to make sure you have the latest value after the straight action
dist_vec.z = tftr.euler_from_quaternion(
tftr.quaternion_slerp(quat0, quat, fraction=1))[2]
while (dist_vec.z -
self.angular_tolerance) > 0.0 and not rospy.is_shutdown():
dist_vec = self.turn_in_place(dist_vec)
self._feedback.vector = dist_vec
self._as.publish_feedback(self._feedback)
rospy.loginfo('%s: Truning to goal %f' %
(self._action_name, self._feedback.x,
self._feedback.y, self._feedback.z))
# Goal should be reached after the operations but this is not helpful
self._result.reached = True
trans, quat = self.get_odom()
pose = Pose()
pose.position = Point(*trans)
pose.orientation = Quaternion(*quat)
self._result.pose = pose
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
class V_search(object):
def __init__(self, height, res):
self.resolution = res # Resolution of the motion
self.motion_height = height # Height of the motion to the ground
# Create search server
self.search_server = SimpleActionServer('search_server',
ExecuteSearchAction,
self.searchCallback, False)
self.server_feedback = ExecuteSearchFeedback()
self.server_result = ExecuteSearchResult()
# Get client from trajectory server
self.trajectory_client = SimpleActionClient(
"approach_server", ExecuteDroneApproachAction)
self.trajectory_client.wait_for_server()
self.next_point = ExecuteDroneApproachGoal(
) # Message to define next position to look for victims
## variables
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
self.odometry = None
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("ground_truth/state", Odometry, self.poseCallback)
# Start trajectory server
self.search_server.start()
def trajectory_feed(self, msg):
'''
Verifies preemption requisitions
'''
if self.search_server.is_preempt_requested():
self.trajectory_client.cancel_goal()
def searchCallback(self, search_area):
'''
Execute a search for vitims into the defined area
'''
x = search_area.x # x size of the area to explore
y = search_area.y # y size of the area to explore
start = search_area.origin # Point to start the search
self.next_point.goal.position.x = start.x # Desired x position
self.next_point.goal.position.y = start.y # Desired y position
theta = 0
# Convert desired angle
q = quaternion_from_euler(0, 0, theta, 'ryxz')
self.next_point.goal.orientation.x = q[0]
self.next_point.goal.orientation.y = q[1]
self.next_point.goal.orientation.z = q[2]
self.next_point.goal.orientation.w = q[3]
x_positions = ceil(x / self.resolution)
y_positions = ceil(y / self.resolution)
x_count = 0 # Counter of steps (in meters traveled)
direction = 1 # Direction of the motion (right, left or up)
trials = 0 # v_search trials
while not rospy.is_shutdown():
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.server_feedback.current_pose = self.odometry
self.odometry_me.release()
if self.search_server.is_preempt_requested():
self.search_server.set_preempted(self.server_result)
return
self.search_server.publish_feedback(self.server_feedback)
self.sonar_me.acquire()
# print("Current height from ground:\n\n{}".format(self.current_height)) # Current distance from ground
h_error = self.motion_height - self.current_height
self.sonar_me.release()
self.odometry_me.acquire()
self.next_point.goal.position.z = self.odometry.position.z + h_error # Desired z position
self.odometry_me.release()
self.trajectory_client.send_goal(self.next_point,
feedback_cb=self.trajectory_feed)
self.trajectory_client.wait_for_result() # Wait for the result
result = self.trajectory_client.get_state(
) # Get the state of the action
# print(result)
if result == GoalStatus.SUCCEEDED:
# Verifies if all the area have been searched
if (self.next_point.goal.position.x == (start.x + x)) and (
(self.next_point.goal.position.y == (start.y + y))):
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.odometry_me.release()
self.search_server.set_succeeded(self.server_result)
return
last_direction = direction
direction = self.square_function(
x_count, 2 * x) # Get the direction of the next step
if last_direction != direction:
# drone moves on y axis
theta = pi / 2
self.next_point.goal.position.y += y / y_positions
elif direction == 1:
# drone moves to the right
theta = 0
self.next_point.goal.position.x += x / x_positions
x_count += x / x_positions
elif direction == -1:
# drone moves to the left
theta = pi
self.next_point.goal.position.x -= x / x_positions
x_count += x / x_positions
# Convert desired angle
q = quaternion_from_euler(0, 0, theta, 'ryxz')
self.next_point.goal.orientation.x = q[0]
self.next_point.goal.orientation.y = q[1]
self.next_point.goal.orientation.z = q[2]
self.next_point.goal.orientation.w = q[3]
elif result == GoalStatus.ABORTED:
trials += 1
if trials == 2:
self.search_server.set_aborted(self.server_result)
return
def square_function(self, x, max_x):
'''
Function to simulate a square wave function
'''
if round(sin(2 * pi * x / max_x), 2) > 0:
return 1
elif round(sin(2 * pi * x / max_x), 2) < 0:
return -1
else:
if round(cos(2 * pi * x / max_x), 2) > 0:
return 1
else:
return -1
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
self.sonar_me.release()
def poseCallback(self, odometry):
'''
Monitor the current position of the robot
'''
self.odometry_me.acquire()
self.odometry = odometry.pose.pose
self.odometry_me.release()
class Approach(object):
def __init__(self, name, takeoff_height):
self.robot_name = name
self.takeoff_height = takeoff_height
# Mutual exclusion
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
# Create trajectory server
self.trajectory_server = SimpleActionServer(
'approach_server', ExecuteDroneApproachAction, self.goCallback,
False)
self.server_feedback = ExecuteDroneApproachFeedback()
self.server_result = ExecuteDroneApproachResult()
# Get client from hector_quadrotor_actions
self.move_client = SimpleActionClient("/{}/action/pose".format(name),
PoseAction)
self.move_client.wait_for_server()
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("/{}/ground_truth/state".format(name), Odometry,
self.poseCallback)
# Subscribe to topic to receive the planned trajectory
rospy.Subscriber("/{}/move_group/display_planned_path".format(name),
DisplayTrajectory, self.planCallback)
#Auxiliary variables
self.trajectory = [] # Array with the trajectory to be executed
self.trajectory_received = False # Flag to signal trajectory received
self.odom_received = False # Flag to signal odom received
self.robot = RobotCommander(
robot_description="{}/robot_description".format(name),
ns="/{}".format(name))
self.display_trajectory_publisher = rospy.Publisher(
'/{}/move_group/display_planned_path'.format(name),
DisplayTrajectory,
queue_size=20)
# Variables for collision callback
self.validity_srv = rospy.ServiceProxy(
'/{}/check_state_validity'.format(name), GetStateValidity)
self.validity_srv.wait_for_service()
self.collision = False
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
#Start move_group
self.move_group = MoveGroup('earth', name)
self.move_group.set_planner(planner_id='RRTConnectkConfigDefault',
attempts=10,
allowed_time=2) #RRTConnectkConfigDefault
self.move_group.set_workspace([XMIN, YMIN, ZMIN, XMAX, YMAX,
ZMAX]) # Set the workspace size
# Get current robot position to define as start planning point
self.current_pose = self.robot.get_current_state()
#Start planningScenePublisher
self.scene_pub = PlanningScenePublisher(name, self.current_pose)
# Start trajectory server
self.trajectory_server.start()
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
self.sonar_me.notify()
self.sonar_me.release()
def poseCallback(self, odometry):
'''
Monitor the current position of the robot
'''
self.odometry_me.acquire()
self.odometry = odometry.pose.pose
# print(self.odometry)
self.odometry_me.release()
self.odom_received = True
def goCallback(self, pose):
'''
Require a plan to go to the desired target and try to execute it 5 time or return erro
'''
self.target = pose.goal
####################################################################
# First takeoff if the drone is close to ground
self.sonar_me.acquire()
while not (self.current_height and self.odometry):
self.sonar_me.wait()
if self.current_height < self.takeoff_height:
self.odometry_me.acquire()
takeoff_pose = PoseGoal()
takeoff_pose.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
takeoff_pose.target_pose.pose.position.x = self.odometry.position.x
takeoff_pose.target_pose.pose.position.y = self.odometry.position.y
takeoff_pose.target_pose.pose.position.z = self.odometry.position.z + self.takeoff_height - self.current_height #Add the heght error to the height
takeoff_pose.target_pose.pose.orientation.x = self.odometry.orientation.x
takeoff_pose.target_pose.pose.orientation.y = self.odometry.orientation.y
takeoff_pose.target_pose.pose.orientation.z = self.odometry.orientation.z
takeoff_pose.target_pose.pose.orientation.w = self.odometry.orientation.w
self.odometry_me.release()
self.move_client.send_goal(takeoff_pose)
self.move_client.wait_for_result()
result = self.move_client.get_state()
if result == GoalStatus.ABORTED:
rospy.logerr("Abort approach! Unable to execute takeoff!")
self.trajectory_server.set_aborted()
return
self.sonar_me.release()
####################################################################
rospy.loginfo("Try to start from [{},{},{}]".format(
self.odometry.position.x, self.odometry.position.y,
self.odometry.position.z))
rospy.loginfo("Try to go to [{},{},{}]".format(self.target.position.x,
self.target.position.y,
self.target.position.z))
self.trials = 0
while self.trials < 5:
rospy.logwarn("Attempt {}".format(self.trials + 1))
if (self.trials > 1):
self.target.position.z += 2 * rd() - 1
result = self.go(self.target)
if (result == 'replan') or (result == 'no_plan'):
self.trials += 1
else:
self.trials = 10
self.collision = False
if result == 'ok':
self.trajectory_server.set_succeeded()
elif (result == 'preempted'):
self.trajectory_server.set_preempted()
else:
self.trajectory_server.set_aborted()
def go(self, target_):
'''
Function to plan and execute the trajectory one time
'''
# Insert goal position on an array
target = []
target.append(target_.position.x)
target.append(target_.position.y)
target.append(target_.position.z)
target.append(target_.orientation.x)
target.append(target_.orientation.y)
target.append(target_.orientation.z)
target.append(target_.orientation.w)
#Define target for move_group
# self.move_group.set_joint_value_target(target)
self.move_group.set_target(target)
self.odometry_me.acquire()
self.current_pose.multi_dof_joint_state.transforms[
0].translation.x = self.odometry.position.x
self.current_pose.multi_dof_joint_state.transforms[
0].translation.y = self.odometry.position.y
self.current_pose.multi_dof_joint_state.transforms[
0].translation.z = self.odometry.position.z
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.x
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.y
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.z
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.w
self.odometry_me.release()
#Set start state
self.move_group.set_start_state(self.current_pose)
# Plan a trajectory till the desired target
plan = self.move_group.plan()
if plan.planned_trajectory.multi_dof_joint_trajectory.points: # Execute only if has points on the trajectory
while (not self.trajectory_received):
rospy.loginfo("Waiting for trajectory!")
rospy.sleep(0.2)
# rospy.loginfo("TRAJECTORY: {}".format(self.trajectory))
#Execute trajectory with action_pose
last_pose = self.trajectory[0]
for pose in self.trajectory:
# Verify preempt call
if self.trajectory_server.is_preempt_requested():
self.move_client.send_goal(last_pose)
self.move_client.wait_for_result()
self.scene_pub.publishScene()
self.trajectory_received = False
self.odom_received = False
return 'preempted'
#Send next pose to move
self.next_pose = pose.target_pose.pose
self.move_client.send_goal(pose,
feedback_cb=self.collisionCallback)
self.move_client.wait_for_result()
result = self.move_client.get_state()
self.scene_pub.publishScene()
# Abort if the drone can not reach the position
if result == GoalStatus.ABORTED:
self.move_client.send_goal(
last_pose) #Go back to the last pose
self.move_client.wait_for_result()
self.trajectory_received = False
self.odom_received = False
return 'aborted'
elif result == GoalStatus.PREEMPTED:
# last_pose.target_pose.pose.position.z += rd() - 0.5
self.move_client.send_goal(
last_pose) #Go back to the last pose
self.move_client.wait_for_result()
self.trajectory_received = False
self.odom_received = False
return 'replan'
elif result == GoalStatus.SUCCEEDED:
self.trials = 0
last_pose = pose
self.server_feedback.current_pose = self.odometry
self.trajectory_server.publish_feedback(self.server_feedback)
# Reset control variables
self.trajectory_received = False
self.odom_received = False
rospy.loginfo("Trajectory is traversed!")
return 'ok'
else:
rospy.logerr("Trajectory is empty. Planning was unsuccessful.")
return 'no_plan'
def planCallback(self, msg):
'''
Receive planned trajectories and insert it into an array of waypoints
'''
if (not self.odom_received):
return
# Variable to calculate the distance difference between 2 consecutive points
last_pose = PoseGoal()
last_pose.target_pose.pose.position.x = self.odometry.position.x
last_pose.target_pose.pose.position.y = self.odometry.position.y
last_pose.target_pose.pose.position.z = self.odometry.position.z
last_pose.target_pose.pose.orientation.x = self.odometry.orientation.x
last_pose.target_pose.pose.orientation.y = self.odometry.orientation.y
last_pose.target_pose.pose.orientation.z = self.odometry.orientation.z
last_pose.target_pose.pose.orientation.w = self.odometry.orientation.w
self.trajectory = []
last_motion_theta = 0
for t in msg.trajectory:
for point in t.multi_dof_joint_trajectory.points:
waypoint = PoseGoal()
waypoint.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
waypoint.target_pose.pose.position.x = point.transforms[
0].translation.x
waypoint.target_pose.pose.position.y = point.transforms[
0].translation.y
waypoint.target_pose.pose.position.z = point.transforms[
0].translation.z
# Orientate the robot always to the motion direction
delta_x = point.transforms[
0].translation.x - last_pose.target_pose.pose.position.x
delta_y = point.transforms[
0].translation.y - last_pose.target_pose.pose.position.y
motion_theta = atan2(delta_y, delta_x)
last_motion_theta = motion_theta
# Make the robot orientation fit with the motion orientation if the movemente on xy is bigger than RESOLUTION
# if (abs(delta_x) > RESOLUTION) or (abs(delta_y) > RESOLUTION):
q = quaternion_from_euler(0, 0, motion_theta)
waypoint.target_pose.pose.orientation.x = q[0]
waypoint.target_pose.pose.orientation.y = q[1]
waypoint.target_pose.pose.orientation.z = q[2]
waypoint.target_pose.pose.orientation.w = q[3]
# else:
# waypoint.target_pose.pose.orientation.x = point.transforms[0].rotation.x
# waypoint.target_pose.pose.orientation.y = point.transforms[0].rotation.y
# waypoint.target_pose.pose.orientation.z = point.transforms[0].rotation.z
# waypoint.target_pose.pose.orientation.w = point.transforms[0].rotation.w
# Add a rotation inplace if next position has an angle difference bigger than 45
if abs(motion_theta - last_motion_theta) > 0.785:
last_pose.target_pose.pose.orientation = waypoint.target_pose.pose.orientation
self.trajectory.append(last_pose)
last_pose = copy.copy(
waypoint) # Save pose to calc the naxt delta
last_motion_theta = motion_theta
self.trajectory.append(waypoint)
#Insert a last point to ensure that the robot end at the right position
waypoint = PoseGoal()
waypoint.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
waypoint.target_pose.pose.position.x = point.transforms[
0].translation.x
waypoint.target_pose.pose.position.y = point.transforms[
0].translation.y
waypoint.target_pose.pose.position.z = point.transforms[
0].translation.z
waypoint.target_pose.pose.orientation.x = point.transforms[
0].rotation.x
waypoint.target_pose.pose.orientation.y = point.transforms[
0].rotation.y
waypoint.target_pose.pose.orientation.z = point.transforms[
0].rotation.z
waypoint.target_pose.pose.orientation.w = point.transforms[
0].rotation.w
self.trajectory.append(waypoint)
self.trajectory_received = True
def collisionCallback(self, feedback):
'''
This callback runs on every feedback message received
'''
validity_msg = GetStateValidityRequest(
) # Build message to verify collision
validity_msg.group_name = PLANNING_GROUP
if self.next_pose and (not self.collision):
self.odometry_me.acquire()
x = self.odometry.position.x
y = self.odometry.position.y
z = self.odometry.position.z
# Distance between the robot and the next position
dist = sqrt((self.next_pose.position.x - x)**2 +
(self.next_pose.position.y - y)**2 +
(self.next_pose.position.z - z)**2)
# Pose to verify collision
pose = Transform()
pose.rotation.x = self.odometry.orientation.x
pose.rotation.y = self.odometry.orientation.y
pose.rotation.z = self.odometry.orientation.z
pose.rotation.w = self.odometry.orientation.w
self.odometry_me.release()
#Verify possible collisions on diferent points between the robot and the goal point
# rospy.logerr("\n\n\nCOLLISION CALLBACK: ")
# rospy.logerr(dist)
for d in arange(RESOLUTION, dist + 0.5, RESOLUTION):
pose.translation.x = (self.next_pose.position.x -
x) * (d / dist) + x
pose.translation.y = (self.next_pose.position.y -
y) * (d / dist) + y
pose.translation.z = (self.next_pose.position.z -
z) * (d / dist) + z
self.current_pose.multi_dof_joint_state.transforms[
0] = pose # Insert the correct odometry value
validity_msg.robot_state = self.current_pose
# Call service to verify collision
collision_res = self.validity_srv.call(validity_msg)
# print("\nCollision response:")
# print(collision_res)
# Check if robot is in collision
if not collision_res.valid:
# print(validity_msg)
rospy.logerr('Collision in front [x:{} y:{} z:{}]'.format(
pose.translation.x, pose.translation.y,
pose.translation.z))
rospy.logerr('Current pose [x:{} y:{} z:{}]'.format(
x, y, z))
# print(collision_res)
self.move_client.cancel_goal()
self.collision = True
return
class CModelActionController(object):
def __init__(self, activate=True):
self._ns = rospy.get_namespace()
# Read configuration parameters
self._fb_rate = read_parameter(
self._ns + 'gripper_action_controller/publish_rate', 60.0)
self._min_gap = read_parameter(
self._ns + 'gripper_action_controller/min_gap', 0.0)
self._min_gap_counts = read_parameter(
self._ns + 'gripper_action_controller/min_gap_counts', 230.)
self._max_gap = read_parameter(
self._ns + 'gripper_action_controller/max_gap', 0.085)
self._min_speed = read_parameter(
self._ns + 'gripper_action_controller/min_speed', 0.013)
self._max_speed = read_parameter(
self._ns + 'gripper_action_controller/max_speed', 0.1)
self._min_force = read_parameter(
self._ns + 'gripper_action_controller/min_force', 40.0)
self._max_force = read_parameter(
self._ns + 'gripper_action_controller/max_force', 100.0)
# Configure and start the action server
self._status = CModelStatus()
self._name = self._ns + 'gripper_action_controller'
self._server = SimpleActionServer(self._name,
CModelCommandAction,
execute_cb=self._execute_cb,
auto_start=False)
self.js_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
rospy.Subscriber('status', CModelStatus, self._status_cb, queue_size=1)
self._cmd_pub = rospy.Publisher('command', CModelCommand, queue_size=1)
working = True
if activate and not self._ready():
rospy.sleep(2.0)
working = self._activate()
if not working:
return
self._server.start()
rospy.logdebug('%s: Started' % self._name)
def _preempt(self):
self._stop()
rospy.loginfo('%s: Preempted' % self._name)
self._server.set_preempted()
def _status_cb(self, msg):
self._status = msg
# Publish the joint_states for the gripper
js_msg = JointState()
js_msg.header.stamp = rospy.Time.now()
js_msg.name.append('robotiq_85_left_knuckle_joint')
js_msg.position.append(0.8 * self._status.gPO / self._min_gap_counts)
self.js_pub.publish(js_msg)
def _execute_cb(self, goal):
success = True
# Check that the gripper is active. If not, activate it.
if not self._ready():
if not self._activate():
rospy.logwarn(
'%s could not accept goal because the gripper is not yet active'
% self._name)
return
# check that preempt has not been requested by the client
if self._server.is_preempt_requested():
self._preempt()
return
# Clip the goal
position = np.clip(goal.position, self._min_gap, self._max_gap)
velocity = np.clip(goal.velocity, self._min_speed, self._max_speed)
force = np.clip(goal.force, self._min_force, self._max_force)
# Send the goal to the gripper and feedback to the action client
rate = rospy.Rate(self._fb_rate)
rospy.logdebug('%s: Moving gripper to position: %.3f ' %
(self._name, position))
feedback = CModelCommandFeedback()
while not self._reached_goal(position):
self._goto_position(position, velocity, force)
if rospy.is_shutdown() or self._server.is_preempt_requested():
self._preempt()
return
feedback.position = self._get_position()
feedback.stalled = self._stalled()
feedback.reached_goal = self._reached_goal(position)
self._server.publish_feedback(feedback)
rate.sleep()
if self._stalled():
break
rospy.logdebug('%s: Succeeded' % self._name)
result = CModelCommandResult()
result.position = self._get_position()
result.stalled = self._stalled()
result.reached_goal = self._reached_goal(position)
self._server.set_succeeded(result)
def _activate(self, timeout=5.0):
command = CModelCommand()
command.rACT = 1
command.rGTO = 1
command.rSP = 255
command.rFR = 150
start_time = rospy.get_time()
while not self._ready():
if rospy.is_shutdown():
self._preempt()
return False
if rospy.get_time() - start_time > timeout:
rospy.logwarn('Failed to activated gripper in ns [%s]' %
(self._ns))
return False
self._cmd_pub.publish(command)
rospy.sleep(0.1)
rospy.loginfo('Successfully activated gripper in ns [%s]' % (self._ns))
return True
def _get_position(self):
gPO = self._status.gPO
pos = np.clip((self._max_gap - self._min_gap) /
(-self._min_gap_counts) * (gPO - self._min_gap_counts),
self._min_gap, self._max_gap)
return pos
def _goto_position(self, pos, vel, force):
"""
Goto position with desired force and velocity
@type pos: float
@param pos: Gripper width in meters
@type vel: float
@param vel: Gripper speed in m/s
@type force: float
@param force: Gripper force in N
"""
command = CModelCommand()
command.rACT = 1
command.rGTO = 1
command.rPR = int(
np.clip((-self._min_gap_counts) / (self._max_gap - self._min_gap) *
(pos - self._min_gap) + self._min_gap_counts, 0,
self._min_gap_counts))
command.rSP = int(
np.clip((255) / (self._max_speed - self._min_speed) *
(vel - self._min_speed), 0, 255))
command.rFR = int(
np.clip((255) / (self._max_force - self._min_force) *
(force - self._min_force), 0, 255))
self._cmd_pub.publish(command)
def _moving(self):
return self._status.gGTO == 1 and self._status.gOBJ == 0
def _reached_goal(self, goal, tol=0.003):
return (abs(goal - self._get_position()) < tol)
def _ready(self):
return self._status.gSTA == 3 and self._status.gACT == 1
def _stalled(self):
return self._status.gOBJ == 1 or self._status.gOBJ == 2
def _stop(self):
command = CModelCommand()
command.rACT = 1
command.rGTO = 0
self._cmd_pub.publish(command)
rospy.logdebug('Stopping gripper in ns [%s]' % (self._ns))
class PathExecutor:
goal_index = 0
reached_all_nodes = True
def __init__(self):
rospy.loginfo('__init__ start')
# create a node
rospy.init_node(NODE)
# Action server to receive goals
self.path_server = SimpleActionServer('/path_executor/execute_path', ExecutePathAction,
self.handle_path, auto_start=False)
self.path_server.start()
# publishers & clients
self.visualization_publisher = rospy.Publisher('/path_executor/current_path', Path)
# get parameters from launch file
self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True)
# action server based on use_obstacle_avoidance
if self.use_obstacle_avoidance == False:
self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction)
else:
self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction)
self.goal_client.wait_for_server()
# other fields
self.goal_index = 0
self.executePathGoal = None
self.executePathResult = ExecutePathResult()
def handle_path(self, paramExecutePathGoal):
'''
Action server callback to handle following path in succession
'''
rospy.loginfo('handle_path')
self.goal_index = 0
self.executePathGoal = paramExecutePathGoal
self.executePathResult = ExecutePathResult()
if self.executePathGoal is not None:
self.visualization_publisher.publish(self.executePathGoal.path)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
if not self.path_server.is_active():
return
if self.path_server.is_preempt_requested():
rospy.loginfo('Preempt requested...')
# Stop bug2
self.goal_client.cancel_goal()
# Stop path_server
self.path_server.set_preempted()
return
if self.goal_index < len(self.executePathGoal.path.poses):
moveto_goal = MoveToGoal()
moveto_goal.target_pose = self.executePathGoal.path.poses[self.goal_index]
self.goal_client.send_goal(moveto_goal, done_cb=self.handle_goal,
feedback_cb=self.handle_goal_preempt)
# Wait for result
while self.goal_client.get_result() is None:
if self.path_server.is_preempt_requested():
self.goal_client.cancel_goal()
else:
rospy.loginfo('Done processing goals')
self.goal_client.cancel_goal()
self.path_server.set_succeeded(self.executePathResult, 'Done processing goals')
return
rate.sleep()
self.path_server.set_aborted(self.executePathResult, 'Aborted. The node has been killed.')
def handle_goal(self, state, result):
'''
Handle goal events (succeeded, preempted, aborted, unreachable, ...)
Send feedback message
'''
feedback = ExecutePathFeedback()
feedback.pose = self.executePathGoal.path.poses[self.goal_index]
# state is GoalStatus message as shown here:
# http://docs.ros.org/diamondback/api/actionlib_msgs/html/msg/GoalStatus.html
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Succeeded finding goal %d", self.goal_index + 1)
self.executePathResult.visited.append(True)
feedback.reached = True
else:
rospy.loginfo("Failed finding goal %d", self.goal_index + 1)
self.executePathResult.visited.append(False)
feedback.reached = False
if not self.executePathGoal.skip_unreachable:
rospy.loginfo('Failed finding goal %d, not skipping...', self.goal_index + 1)
# Stop bug2
self.goal_client.cancel_goal()
# Stop path_server
self.path_server.set_succeeded(self.executePathResult,
'Unreachable goal in path. Done processing goals.')
#self.path_server.set_preempted()
#return
self.path_server.publish_feedback(feedback)
self.goal_index = self.goal_index + 1
def handle_goal_preempt(self, state):
'''
Callback for goal_client to check for preemption from path_server
'''
if self.path_server.is_preempt_requested():
self.goal_client.cancel_goal()
class ErraticBaseActionServer():
def __init__(self):
self.base_frame = '/base_footprint'
self.move_client = SimpleActionClient('move_base', MoveBaseAction)
self.move_client.wait_for_server()
self.tf = tf.TransformListener()
self.result = ErraticBaseResult()
self.feedback = ErraticBaseFeedback()
self.server = SimpleActionServer(NAME, ErraticBaseAction, self.execute_callback, auto_start=False)
self.server.start()
rospy.loginfo("%s: Ready to accept goals", NAME)
def transform_target_point(self, point):
self.tf.waitForTransform(self.base_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
return self.tf.transformPoint(self.base_frame, point)
def move_to(self, target_pose):
goal = MoveBaseGoal()
goal.target_pose = target_pose
goal.target_pose.header.stamp = rospy.Time.now()
self.move_client.send_goal(goal=goal, feedback_cb=self.move_base_feedback_cb)
while not self.move_client.wait_for_result(rospy.Duration(0.01)):
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.move_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.move_client.get_state()
def move_base_feedback_cb(self, fb):
self.feedback.base_position = fb.base_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
def get_vicinity_target(self, target_pose, vicinity_range):
vicinity_pose = PoseStamped()
# transform target to base_frame reference
target_point = PointStamped()
target_point.header.frame_id = target_pose.header.frame_id
target_point.point = target_pose.pose.position
self.tf.waitForTransform(self.base_frame, target_pose.header.frame_id, rospy.Time(), rospy.Duration(5.0))
target = self.tf.transformPoint(self.base_frame, target_point)
rospy.logdebug("%s: Target at (%s, %s, %s)", NAME, target.point.x, target.point.y, target.point.z)
# find distance to point
dist = math.sqrt(math.pow(target.point.x, 2) + math.pow(target.point.y, 2))
if (dist < vicinity_range):
# if already within range, then no need to move
vicinity_pose.pose.position.x = 0.0
vicinity_pose.pose.position.y = 0.0
else:
# normalize vector pointing from source to target
target.point.x /= dist
target.point.y /= dist
# scale normal vector to within vicinity_range distance from target
target.point.x *= (dist - vicinity_range)
target.point.y *= (dist - vicinity_range)
# add scaled vector to source
vicinity_pose.pose.position.x = target.point.x + 0.0
vicinity_pose.pose.position.y = target.point.y + 0.0
# set orientation
ori = Quaternion()
yaw = math.atan2(target.point.y, target.point.x)
(ori.x, ori.y, ori.z, ori.w) = tf.transformations.quaternion_from_euler(0, 0, yaw)
vicinity_pose.pose.orientation = ori
# prep header
vicinity_pose.header = target_pose.header
vicinity_pose.header.frame_id = self.base_frame
rospy.logdebug("%s: Moving to (%s, %s, %s)", NAME, vicinity_pose.pose.position.x, vicinity_pose.pose.position.y, vicinity_pose.pose.position.z)
return vicinity_pose
def execute_callback(self, goal):
rospy.loginfo("%s: Executing move base", NAME)
move_base_result = None
if goal.vicinity_range == 0.0:
# go to exactly
move_base_result = self.move_to(goal.target_pose)
else:
# go near (within vicinity_range meters)
vicinity_target_pose = self.get_vicinity_target(goal.target_pose, goal.vicinity_range)
move_base_result = self.move_to(vicinity_target_pose)
# check results
if (move_base_result == GoalStatus.SUCCEEDED):
rospy.loginfo("%s: Succeeded", NAME)
self.result.base_position = self.feedback.base_position
self.server.set_succeeded(self.result)
elif (move_base_result == GoalStatus.PREEMPTED):
rospy.loginfo("%s: Preempted", NAME)
self.server.set_preempted()
else:
rospy.loginfo("%s: Aborted", NAME)
self.server.set_aborted()
class GoalServer(object):
def __init__(self, name, predicate, parameters):
rospy.loginfo("Starting %s ..." % name)
self._as = SimpleActionServer(name,
GoalServerAction,
execute_cb=self.execute_cb,
auto_start=False)
self.planning_goal = predicate + "__" + "__".join(parameters)
print self.planning_goal
self.client = SimpleActionClient("/kcl_rosplan/start_planning",
PlanAction)
self.client.wait_for_server()
self._as.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
self.__call_service("/kcl_rosplan/clear_goals", Empty, EmptyRequest())
self.add_goal(self.planning_goal)
tries = goal.tries if goal.tries > 1 else float("inf")
cnt = 0
while self.client.send_goal_and_wait(PlanGoal()) != GoalStatus.SUCCEEDED \
and cnt < tries \
and not self._as.is_preempt_requested() \
and not rospy.is_shutdown():
self._as.publish_feedback(GoalServerFeedback(cnt))
rospy.sleep(goal.timeout)
cnt += 1
else:
self._as.set_aborted()
return
self._as.set_succeeded()
def add_goal(self, goal):
srv_name = "/kcl_rosplan/update_knowledge_base_array"
req = KnowledgeUpdateServiceArrayRequest()
req.update_type = req.ADD_GOAL
goal = [goal] if not isinstance(goal, list) else goal
for p in goal:
cond = p.split("__")
rospy.loginfo("Adding %s" % str(p))
tp = self._get_predicate_details(
cond[0]).predicate.typed_parameters
if len(tp) != len(cond[1:]):
rospy.logerr(
"Fact '%s' should have %s parameters but has only %s as parsed from: '%s'"
% (cond[0], len(tp), len(cond[1:])))
return
req.knowledge.append(
KnowledgeItem(knowledge_type=KnowledgeItem.FACT,
attribute_name=cond[0],
values=[
KeyValue(key=str(k.key), value=str(v))
for k, v in zip(tp, cond[1:])
]))
while not rospy.is_shutdown():
try:
self.__call_service(srv_name, KnowledgeUpdateServiceArray, req)
except rospy.ROSInterruptException:
rospy.logerr("Communication with '%s' interrupted. Retrying." %
srv_name)
rospy.sleep(1.)
else:
return
def __call_service(self, srv_name, srv_type, req):
while not rospy.is_shutdown():
try:
s = rospy.ServiceProxy(srv_name, srv_type)
s.wait_for_service(timeout=1.)
except rospy.ROSException:
rospy.logwarn(
"Could not communicate with '%s' service. Retrying in 1 second."
% srv_name)
rospy.sleep(1.)
else:
return s(req)
def _get_predicate_details(self, name):
srv_name = "/kcl_rosplan/get_domain_predicate_details"
while not rospy.is_shutdown():
try:
return self.__call_service(srv_name,
GetDomainPredicateDetailsService,
name)
except rospy.ROSInterruptException:
rospy.logerr("Communication with '%s' interrupted. Retrying." %
srv_name)
rospy.sleep(1.)
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 PickUpActionServer():
def __init__(self):
# Initialize new node
rospy.init_node(NAME)#, anonymous=False)
#initialize the clients to interface with
self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
self.move_client.wait_for_server()
self.arm_client.wait_for_server()
self.gripper_client.wait_for_server()
# Initialize tf listener (remove?)
self.tf = tf.TransformListener()
# Initialize erratic base action server
self.result = SmartArmGraspResult()
self.feedback = SmartArmGraspFeedback()
self.server = SimpleActionServer(NAME, SmartArmGraspAction, self.execute_callback)
#define the offsets
# These offsets were determines expirmentally using the simulator
# They were tested using points stamped with /map
self.xOffset = 0.025
self.yOffset = 0.0
self.zOffset = 0.12 #.05 # this does work!
rospy.loginfo("%s: Pick up Action Server is ready to accept goals", NAME)
rospy.loginfo("%s: Offsets are [%f, %f, %f]", NAME, self.xOffset, self.yOffset, self.zOffset )
def move_to(self, frame_id, position, orientation, vicinity=0.0):
goal = ErraticBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = frame_id
goal.target_pose.pose.position = position
goal.target_pose.pose.orientation = orientation
goal.vicinity_range = vicinity
self.move_client.send_goal(goal)
#print "going into loop"
while (not self.move_client.wait_for_result(rospy.Duration(0.01))):
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.move_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.move_client.get_state()
#(almost) blatent copy / past from wubble_head_action.py. Err, it's going to the wrong position
def transform_target_point(self, point, frameId):
#rospy.loginfo("%s: got %s %s %s %s", NAME, point.header.frame_id, point.point.x, point.point.y, point.point.z)
# Wait for tf info (time-out in 5 seconds)
self.tf.waitForTransform(frameId, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
# Transform target point & retrieve the pan angle
return self.tf.transformPoint(frameId, point)
#UNUSED
def move_base_feedback_cb(self, fb):
self.feedback.base_position = fb.base_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
# This moves the arm into a positions based on angles (in rads)
# It depends on the sources code in wubble_actions
def move_arm(self, shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate):
goal = SmartArmGoal()
goal.target_joints = [shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate]
self.arm_client.send_goal(goal, None, None, self.arm_position_feedback_cb)
self.arm_client.wait_for_goal_to_finish()
while not self.arm_client.wait_for_result(rospy.Duration(0.01)) :
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.arm_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.arm_client.get_state()
# This moves the wrist of the arm to the x, y, z coordinates
def reach_at(self, frame_id, x, y, z):
goal = SmartArmGoal()
goal.target_point = PointStamped()
goal.target_point.header.frame_id = frame_id
goal.target_point.point.x = x
goal.target_point.point.y = y
goal.target_point.point.z = z
rospy.loginfo("%s: Original point is '%s' [%f, %f, %f]", NAME, frame_id,\
goal.target_point.point.x, goal.target_point.point.y, goal.target_point.point.z)
goal.target_point = self.transform_target_point(goal.target_point, '/arm_base_link');
rospy.loginfo("%s: Transformed point is '/armbaselink' [%f, %f, %f]", NAME, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
goal.target_point.point.x = goal.target_point.point.x + self.xOffset
goal.target_point.point.y = goal.target_point.point.y + self.yOffset
goal.target_point.point.z = goal.target_point.point.z + self.zOffset
rospy.loginfo("%s: Transformed and Offset point is '/armbaselink' [%f, %f, %f]", NAME, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
self.arm_client.send_goal(goal, None, None, self.arm_position_feedback_cb)
self.arm_client.wait_for_goal_to_finish()
while not self.arm_client.wait_for_result(rospy.Duration(0.01)) :
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.arm_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.arm_client.get_state()
#This method is used passes updates from arm positions actions to the feedback
#of this server
def arm_position_feedback_cb(self, fb):
self.feedback.arm_position = fb.arm_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
#This moves the gripper to the given angles
def move_gripper(self, left_finger, right_finger):
goal = SmartArmGripperGoal()
goal.target_joints = [left_finger, right_finger]
self.gripper_client.send_goal(goal, None, None, self.gripper_position_feedback_cb)
#gripper_client.wait_for_goal_to_finish()
while not self.gripper_client.wait_for_result(rospy.Duration(0.01)) :
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.gripper_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.gripper_client.get_state()
#This method is used passes updates from arm positions actions to the feedback
#of this server
def gripper_position_feedback_cb(self, fb):
self.feedback.gripper_position = fb.gripper_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
#This method sets the results of the goal to the last feedback values
def set_results(self, success):
self.result.success = success
self.result.arm_position = self.feedback.arm_position
self.result.gripper_position = self.feedback.gripper_position
self.server.set_succeeded(self.result)
#This is the callback function that is executed whenever the server
#recieves a request
def execute_callback(self, goal):
rospy.loginfo("%s: Executing Grasp Action [%s, %f, %f, %f]", NAME, \
goal.target_point.header.frame_id, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
rospy.loginfo( "%s: Moving the arm to the cobra pose", NAME)
#move the arm into the cobra position
result = self.move_arm(0.0, 1.972222, -1.972222, 0.0)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: 1st Move Arm (to cobra pose) Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: 1st Move Arm (to cobra pose) Failed", NAME)
self.set_results(False)
return
position = Point(x = goal.target_point.point.x, y = goal.target_point.point.y)
orientation = Quaternion(w=1.0)
self.move_to('/map', position, orientation, 0.5)
self.move_to('/map', position, orientation, 0.2)
rospy.sleep(0.2)
rospy.loginfo( "%s: Opening gripper", NAME)
#open the gripper
result = self.move_gripper(0.2, -0.2)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: Open Gripper Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: Open Gripper Failed", NAME)
self.set_results(False)
return
rospy.loginfo( "%s: Moving the arm to the object", NAME)
#move the arm to the correct posisions
result = self.reach_at(goal.target_point.header.frame_id, \
goal.target_point.point.x, \
goal.target_point.point.y, \
goal.target_point.point.z)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: 2nd Move Arm (to object) Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: 2nd Move Arm (to object) Failed", NAME)
self.set_results(False)
return
rospy.loginfo( "%s: Moving the elbow joint to the cobra pose", NAME)
#move the arm into the cobra position
result = self.move_arm(self.feedback.arm_position[0], self.feedback.arm_position[1], \
-3.14 / 2 - self.feedback.arm_position[1], self.feedback.arm_position[3])
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: Moving the elbow joint Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: Moving the elbow joint Failed", NAME)
self.set_results(False)
return
rospy.loginfo( "%s: Closing gripper", NAME)
#close the gripper
result = self.move_gripper(-1.5, 1.5)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: Close Gripper Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
#this actions 'succeeds' if it times out
rospy.loginfo( "%s: Moving the arm to the cobra pose", NAME)
#move the arm into the cobra position
result = self.move_arm(0.0, 1.972222, -1.972222, 0.0)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: 3rd Move Arm (to cobra pose) Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: 3rd Move Arm (to cobra pose) Failed", NAME)
self.set_results(False)
return
#action has succeeded
rospy.loginfo("%s: Grasp Action Succeed [%s, %f, %f, %f]", NAME, \
goal.target_point.header.frame_id, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
self.set_results(True)
"""
class SmartArmActionServer():
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 4 # Number of joints to manage
self.ERROR_THRESHOLD = 0.15 # Report success if error reaches below threshold
self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME + 'server', anonymous=True)
# Initialize publisher & subscriber for shoulder pan
self.shoulder_pan_frame = 'arm_shoulder_pan_link'
self.shoulder_pan = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.shoulder_pan_pub = rospy.Publisher('shoulder_pan_controller/command', Float64)
rospy.Subscriber('shoulder_pan_controller/state', JointControllerState, self.read_shoulder_pan)
rospy.wait_for_message('shoulder_pan_controller/state', JointControllerState)
# Initialize publisher & subscriber for shoulder tilt
self.shoulder_tilt_frame = 'arm_shoulder_tilt_link'
self.shoulder_tilt = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.shoulder_tilt_pub = rospy.Publisher('shoulder_tilt_controller/command', Float64)
rospy.Subscriber('shoulder_tilt_controller/state', JointControllerState, self.read_shoulder_tilt)
rospy.wait_for_message('shoulder_tilt_controller/state', JointControllerState)
# Initialize publisher & subscriber for elbow tilt
self.elbow_tilt_frame = 'arm_elbow_tilt_link'
self.elbow_tilt = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.elbow_tilt_pub = rospy.Publisher('elbow_tilt_controller/command', Float64)
rospy.Subscriber('elbow_tilt_controller/state', JointControllerState, self.read_elbow_tilt)
rospy.wait_for_message('elbow_tilt_controller/state', JointControllerState)
# Initialize publisher & subscriber for shoulder tilt
self.wrist_rotate_frame = 'arm_wrist_rotate_link'
self.wrist_rotate = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.wrist_rotate_pub = rospy.Publisher('wrist_rotate_controller/command', Float64)
rospy.Subscriber('wrist_rotate_controller/state', JointControllerState, self.read_wrist_rotate)
rospy.wait_for_message('wrist_rotate_controller/state', JointControllerState)
# Initialize tf listener
self.tf = tf.TransformListener()
# Initialize joints action server
self.result = SmartArmResult()
self.feedback = SmartArmFeedback()
self.feedback.arm_position = [self.shoulder_pan.process_value, self.shoulder_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_rotate.process_value]
self.server = SimpleActionServer(NAME, SmartArmAction, self.execute_callback)
# Reset arm position
rospy.sleep(1)
self.reset_arm_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
def reset_arm_position(self):
# reset arm to cobra position
self.shoulder_pan_pub.publish(0.0)
self.shoulder_tilt_pub.publish(1.972222)
self.elbow_tilt_pub.publish(-1.972222)
self.wrist_rotate_pub.publish(0.0)
rospy.sleep(12)
def read_shoulder_pan(self, pan_data):
self.shoulder_pan = pan_data
self.has_latest_shoulder_pan = True
def read_shoulder_tilt(self, tilt_data):
self.shoulder_tilt = tilt_data
self.has_latest_shoulder_tilt = True
def read_elbow_tilt(self, tilt_data):
self.elbow_tilt = tilt_data
self.has_latest_elbow_tilt = True
def read_wrist_rotate(self, rotate_data):
self.wrist_rotate = rotate_data
self.has_latest_wrist_rotate = True
def wait_for_latest_controller_states(self, timeout):
self.has_latest_shoulder_pan = False
self.has_latest_shoulder_tilt = False
self.has_latest_elbow_tilt = False
self.has_latest_wrist_rotate = False
r = rospy.Rate(100)
start = rospy.Time.now()
while (self.has_latest_shoulder_pan == False or self.has_latest_shoulder_tilt == False or \
self.has_latest_elbow_tilt == False or self.has_latest_wrist_rotate == False) and \
(rospy.Time.now() - start < timeout):
r.sleep()
def transform_target_point(self, point):
rospy.loginfo("%s: Retrieving IK solutions", NAME)
rospy.wait_for_service('smart_arm_ik_service', 10)
ik_service = rospy.ServiceProxy('smart_arm_ik_service', SmartArmIK)
resp = ik_service(point)
if (resp and resp.success):
return resp.solutions[0:4]
else:
raise Exception, "Unable to obtain IK solutions."
def execute_callback(self, goal):
r = rospy.Rate(100)
self.result.success = True
self.result.arm_position = [self.shoulder_pan.process_value, self.shoulder_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_rotate.process_value]
rospy.loginfo("%s: Executing move arm", NAME)
# Initialize target joints
target_joints = list()
for i in range(self.JOINTS_COUNT):
target_joints.append(0.0)
# Retrieve target joints from goal
if (len(goal.target_joints) > 0):
for i in range(min(len(goal.target_joints), len(target_joints))):
target_joints[i] = goal.target_joints[i]
else:
try:
# Convert target point to target joints (find an IK solution)
target_joints = self.transform_target_point(goal.target_point)
except (Exception, tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("%s: Aborted: IK Transform Failure", NAME)
self.result.success = False
self.server.set_aborted()
return
# Publish goal to controllers
self.shoulder_pan_pub.publish(target_joints[0])
self.shoulder_tilt_pub.publish(target_joints[1])
self.elbow_tilt_pub.publish(target_joints[2])
self.wrist_rotate_pub.publish(target_joints[3])
# Initialize loop variables
start_time = rospy.Time.now()
while (math.fabs(target_joints[0] - self.shoulder_pan.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[1] - self.shoulder_tilt.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[2] - self.elbow_tilt.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[3] - self.wrist_rotate.process_value) > self.ERROR_THRESHOLD):
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
break
# Publish current arm position as feedback
self.feedback.arm_position = [self.shoulder_pan.process_value, self.shoulder_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_rotate.process_value]
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > self.TIMEOUT_THRESHOLD):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
break
r.sleep()
if (self.result.success):
rospy.loginfo("%s: Goal Completed", NAME)
self.wait_for_latest_controller_states(rospy.Duration(2.0))
self.result.arm_position = [self.shoulder_pan.process_value, self.shoulder_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_rotate.process_value]
self.server.set_succeeded(self.result)
class AudioFilePlayer(object):
def __init__(self):
rospy.loginfo("Initializing AudioFilePlayer...")
self.current_playing_process = None
self.afp_as = SimpleActionServer(rospy.get_name(),
AudioFilePlayAction,
self.as_cb,
auto_start=False)
self.afp_sub = rospy.Subscriber('~play',
String,
self.topic_cb,
queue_size=1)
# By default this node plays files using the aplay command
# Feel free to use any other command or flags
# by using the params provided
self.command = rospy.get_param('~command', 'play')
self.flags = rospy.get_param('~flags', '')
self.wrap_file_path = rospy.get_param('~wrap_file_path', True)
self.feedback_rate = rospy.get_param('~feedback_rate', 10)
self.afp_as.start()
# Needs to be done after start
self.afp_as.register_preempt_callback(self.as_preempt_cb)
rospy.loginfo(
"Done, playing files from action server or topic interface.")
def as_preempt_cb(self):
if self.current_playing_process:
self.current_playing_process.kill()
# Put the AS as cancelled/preempted
res = AudioFilePlayResult()
res.success = False
res.reason = "Got a cancel request."
self.afp_as.set_preempted(res, text="Cancel requested.")
def as_cb(self, goal):
initial_time = time.time()
self.play_audio_file(goal.filepath)
r = rospy.Rate(self.feedback_rate)
while not rospy.is_shutdown(
) and not self.current_playing_process.is_done():
feedback = AudioFilePlayFeedback()
curr_time = time.time()
feedback.elapsed_played_time = rospy.Duration(curr_time -
initial_time)
self.afp_as.publish_feedback(feedback)
r.sleep()
final_time = time.time()
res = AudioFilePlayResult()
if self.current_playing_process.is_succeeded():
res.success = True
res.total_time = rospy.Duration(final_time)
self.afp_as.set_succeeded(res)
else:
if self.afp_as.is_preempt_requested():
return
res.success = False
reason = "stderr: " + self.current_playing_process.get_stderr()
reason += "\nstdout: " + self.current_playing_process.get_stdout()
res.reason = reason
self.afp_as.set_aborted(res)
def topic_cb(self, data):
if self.current_playing_process:
if not self.current_playing_process.is_done():
self.current_playing_process.kill()
self.play_audio_file(data.data)
def play_audio_file(self, audio_file_path):
# Replace any ' or " characters with emptyness to avoid bad usage
audio_file_path = audio_file_path.replace("'", "")
audio_file_path = audio_file_path.replace('"', '')
if self.wrap_file_path:
full_command = self.command + " " + self.flags + " '" + audio_file_path + "'"
else:
full_command = self.command + " " + self.flags + " " + audio_file_path
rospy.loginfo("Playing audio file: " + str(audio_file_path) +
" with command: " + str(full_command))
self.current_playing_process = ShellCmd(full_command)