def run(self):
self.event_type = 'uncontrollable' # Event type selected on Radio
self.enabled_e = [] # List of enabled events
self.param = [] # Parameters of the event
trace = Thread(
target=self.events_trace) # Thread for the tracer monitor
trace.start() # Start event tracer as a thread
while True:
#Get data from the Window
event, values = self.window.Read(timeout=10)
if event in (None,
'Cancel'): # If user closes window or clicks cancel
print('\nCLOSING EVENT INTERFACE ...\n')
break
# New event occured on the Prodct System
if self.new_trace == True:
self.new_trace = False
#Update tracer screen
if self.trace.tail(1).index[0] > 0:
if self.__events[self.trace.tail(1)
['event'].values[0]].is_controllable():
color = 'blue'
else:
color = 'red'
text = self.trace.tail(1).drop(
columns=['enabled_events', 'states']).to_string(
header=False, justify='left')
self.window['tracer'].print(text, text_color=color)
#Update the Automaton Image
try:
self.window.Element("_IMAGE_").update(
filename="output/" + values['option'] + ".png")
except:
pass
if event == 'option':
#Update the Automaton Image
try:
self.window.Element("_IMAGE_").update(
filename="output/" + values['option'] + ".png")
except:
pass
elif event == 'controllable':
#Show list of enabled controllable events
self.window.Element('selected_event').update(
values=self.enabled_e)
self.event_type = 'controllable'
elif event == 'uncontrollable':
#Show list of uncontrollable events
self.window.Element('selected_event').update(
values=self.__not_cont_e)
self.event_type = 'uncontrollable'
if event == 'trigger':
# An event is triggered
if values['controllable'] == True:
trigger_event(
values['selected_event'], self.param
) # Call the execution of the controllable event
else:
# Trigger uncontrollable events
event = values['selected_event']
# Translate non-controllable events to low-level call
ll_event = self.translation_table[(
self.translation_table['high-level'] == event
)]['low-level'].array[0]
topic = self.translation_table[(
self.translation_table['high-level'] == event
)]['topic'].array[0]
# Fake uncontrollable events
if 'erro' in ll_event:
if 'maneuvers/out' in topic:
# Fake maneuver error
move_base_client = SimpleActionClient(
"{}move_base".format(rospy.get_namespace()),
MoveBaseAction) # Get move_base service
move_base_client.wait_for_server()
move_base_client.cancel_all_goals(
) # Cancel the current motion
elif ('gas_sensor/out'
in topic) or ('victim_sensor/out' in topic):
# Fake sensor error
topic = topic.replace(
'/out',
'/in') # Get sensor/in topic to fake a failure
pub = rospy.Publisher("/{}".format(topic),
events_message,
queue_size=10)
msg = events_message()
msg.event = ll_event
pub.publish(msg) # Publish to the sensor topic
elif 'bat_' in event:
# Fake battery state change
topic = topic.replace(
'/out', '/in') # Get battery_monitor/in topic
pub = rospy.Publisher("/{}".format(topic),
events_message,
queue_size=10)
msg = events_message()
msg.event = ll_event
if event == 'bat_OK':
msg.param.append(
60.0
) # At level = 60 the system consider bat_OK
elif event == 'bat_L':
msg.param.append(
30.0
) # At level = 30 the system consider bat_L
elif event == 'bat_LL':
msg.param.append(
9.0) # At level = 9 the system consider bat_LL
pub.publish(
msg) # Publish to the battery_monitor topic
elif 'failure' in ll_event:
# Fake failures
topic = topic.replace(
'/out', '/in') # Get failures_monitor/in topic
pub = rospy.Publisher("/{}".format(topic),
events_message,
queue_size=10)
msg = events_message()
msg.event = ll_event
pub.publish(
msg) # Publish to the failures_monitor topic
elif 'ihm' in topic:
# Fake commander comands
pub = rospy.Publisher("/{}".format(topic),
events_message,
queue_size=10)
msg = events_message()
msg.event = ll_event
pub.publish(msg) # Publish to the IHM/out topic
self.param = [] # Clear param
self.window['param_list'].Update(
values=self.param) # Clear param screen
elif event == 'refresh':
self.window['tracer'].update('')
#Refresh tracer
if not self.trace.empty:
for i in self.trace.iloc[1:].index:
if self.__events[self.trace.at[
i, 'event']].is_controllable():
color = 'blue'
else:
color = 'red'
text = self.trace.loc[[i]].drop(
columns=['enabled_events', 'states']).to_string(
header=False, justify='left')
self.window['tracer'].print(text, text_color=color)
elif event == 'save':
# Save content of tracer into a csv file
filename = values['save']
if filename:
if '.' not in filename:
filename += ".csv"
if '.csv' in filename:
self.trace.drop(columns=['enabled_events', 'states'
]).to_csv(filename)
else:
sg.Popup('Wrong file extension!',
title='Saving failure!')
self.window['save'].update(value='')
elif event == 'add_param':
# Add a new item as parameter for the event
if values['new_param']:
self.param.append(eval(values['new_param']))
self.window['new_param'].update('')
self.window['param_list'].Update(values=self.param)
elif event == 'remove_param':
# Remove an item from the list of parameters
if self.window['param_list'].GetIndexes():
item = self.window['param_list'].GetIndexes()[0]
self.param.pop(item)
self.window['param_list'].Update(values=self.param)
self.window.Close()
def execute_inner(self, userdata):
global ms
self.last_mv = rospy.Time.now()
rospy.loginfo('Beginning Exploration')
client = SimpleActionClient('explore_server', ExploreTaskAction)
rospy.loginfo('WAITING FOR EXPLORE SERVER...')
client.wait_for_server()
rospy.loginfo('EXPLORE SERVER IS NOW UP!')
boundary = PolygonStamped()
boundary.header.frame_id = "map"
boundary.header.stamp = rospy.Time.now()
r = userdata.boundary / 2.0
rospy.loginfo('boundary : {}'.format(r))
x, y, _ = userdata.initial_point
boundary.polygon.points.append(Point(x + r, y + r, 0))
boundary.polygon.points.append(Point(x - r, y + r, 0))
boundary.polygon.points.append(Point(x - r, y - r, 0))
boundary.polygon.points.append(Point(x + r, y - r, 0))
center = PointStamped()
center.header.frame_id = "map"
center.header.stamp = rospy.Time.now()
center.point.x = x
center.point.y = y
center.point.z = 0.0
goal = ExploreTaskGoal()
goal.explore_boundary = boundary
goal.explore_center = center
client.send_goal(goal)
while True:
if rospy.is_shutdown():
exit()
if client.wait_for_result(
rospy.Duration(0.3)): # 0.3 sec. timeout to check for cans
# The exploration node has finished
res = client.get_state()
rospy.loginfo('EXPLORE SERVER STATE:{}'.format(res))
if res == 3: ## SUCCEEDED
# if exploration is complete...
userdata.boundary += 1.0 # explore a larger area
return 'succeeded' # finished! yay!
else:
print client.get_goal_status_text()
print 'explore server failed : {}'.format(res)
# when explore server gives up, can't explore
return 'stuck'
now = rospy.Time.now()
if self.objective == 'discovery':
t = ms.dis_data.time
p = ms.dis_pt()
elif self.objective == 'delivery':
t = ms.del_data.time
p = ms.del_pt()
else:
rospy.logerr('Invalid objective passed to Explore state')
return 'aborted'
# if we're here, exploration is not complete yet...
if t != None: # check initialized
dt = (now - t).to_sec()
discovered = (dt < 2.0) # last seen within the last 2 seconds
if discovered:
# if can was found ...
client.cancel_all_goals()
userdata.destination = p
return 'discovered'
# more than 20 seconds have passed while completely still
# we're probably stuck
if (now - self.last_mv).to_sec() > 20.0:
print 'haven\'t been moving for a while!'
client.cancel_all_goals()
return 'stuck' # bad name... "stuck" would be better
class SerialImuNode(object):
'''Publishes IMU data from serial port'''
def __init__(self):
'''Initilize Serial IMU Node'''
# Initilize Node
rospy.init_node('serial_imu_node')
self._port_name = rospy.get_param('~port', '/dev/rfcomm0')
self._baud = rospy.get_param('~baud', 9600)
self._frame_id = rospy.get_param('~frame_id', 'imu_link')
self._rate = rospy.get_param('~rate', 10) # 10hz
self._movebase_ns = rospy.get_param('~movebase_ns', 'move_base')
self._fetched_thresh = rospy.get_param('~fetched_thresh', 1000)
self._fetched = False
self.message_pub = rospy.Publisher('imu', Imu, queue_size=5)
self.marker_pub = rospy.Publisher('imu/marker', Marker, queue_size=5)
self.sac = SimpleActionClient(self._movebase_ns, MoveBaseAction)
self._goal = MoveBaseGoal()
self._goal.target_pose.header.frame_id = "map"
self._goal.target_pose.header.stamp = rospy.Time.now()
self._goal.target_pose.pose.position.x = 0
self._goal.target_pose.pose.position.x = 0
self._goal.target_pose.pose.orientation.w = 1.0
self.rate = rospy.Rate(self._rate)
while not rospy.is_shutdown():
with serial.Serial(self._port_name, self._baud,
timeout=0.01) as ser:
rospy.loginfo("Connected to: " + ser.portstr)
t = Thread(target=receiving, args=(ser, ))
t.start()
while not rospy.is_shutdown():
values = None
try:
# line = ser.readline()
line = last_received
# print("line: ", line)
values = map(float, line.split(','))
# print("values: ", values)
except:
pass
if values is not None:
if len(values) == 10:
self.update(values)
self.rate.sleep()
if not t.isAlive():
break
stop_receiving = True
t.do_receive = False
t.join()
def update(self, values):
imu_msg = self.update_imu(values)
self.update_marker(imu_msg)
norm = np.linalg.norm(values[7:9])
if (norm > self._fetched_thresh) and (not self._fetched):
self._fetched = True
self.sac.send_goal(self._goal)
if (norm < self._fetched_thresh) and (self._fetched):
self._fetched = False
self.sac.cancel_all_goals()
def update_imu(self, values):
imu_msg = Imu()
imu_msg.header.frame_id = self._frame_id
# print("values: ", values)
imu_msg.header.stamp = rospy.Time.now()
imu_msg.linear_acceleration.x = values[1]
imu_msg.linear_acceleration.y = values[2]
imu_msg.linear_acceleration.z = values[3]
# imu_msg.linear_acceleration_covariance = [0,0,0,0,0,0,0,0,0]
imu_msg.angular_velocity.x = values[4]
imu_msg.angular_velocity.y = values[5]
imu_msg.angular_velocity.z = values[6]
# imu_msg.angular_velocity_covariance = [0,0,0,0,0,0,0,0,0]
imu_msg.orientation.x = values[7]
imu_msg.orientation.y = values[8]
imu_msg.orientation.z = values[9]
# imu_msg.orientation_covariance = [0,0,0,0,0,0,0,0,0]
# print("imu_msg: ", imu_msg)
self.message_pub.publish(imu_msg)
# rospy.loginfo(imu_msg)
return imu_msg
def update_marker(self, imu_msg):
u = [.1, .1, .1]
v = [
imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z
]
q = fromtwovectors(u, v)
marker = Marker()
marker.header.frame_id = self._frame_id
marker.type = marker.ARROW
marker.action = marker.ADD
marker.scale.x = 1
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.orientation.x = q[1]
marker.pose.orientation.y = q[2]
marker.pose.orientation.z = q[3]
marker.pose.orientation.w = q[0]
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
self.marker_pub.publish(marker)
class Navigator(object):
""" Navigation client that is responsible for moving the robot
on the map.
"""
def __init__(self, dispatcher, verbose=False):
self.verbose = verbose
self.dispatcher = dispatcher
self.client = Client(topics.move_base, MoveBaseAction)
self.base_pending = Event()
self.current_pose = PoseStamped()
self.target_pose = PoseStamped()
def cancel_all_goals(self):
log.debug('Waiting for the Navigation action server...')
self.client.wait_for_server()
log.info('Canceling all goals on Navigation.')
self.base_pending.clear()
self.client.cancel_all_goals()
sleep(3)
def move_base(self, target):
""" Move base to a point of interest.
:param :target A PoseStamped point of interest.
"""
roll, pitch, yaw = euler_from_quaternion([
target.pose.orientation.x, target.pose.orientation.y,
target.pose.orientation.z, target.pose.orientation.w
])
transformend_orientation = quaternion_from_euler(roll, pitch, yaw + pi)
target.pose.orientation.x = transformend_orientation[0]
target.pose.orientation.y = transformend_orientation[1]
target.pose.orientation.z = transformend_orientation[2]
target.pose.orientation.w = transformend_orientation[3]
target.pose.position.z = 0
goal = MoveBaseGoal(target_pose=target)
self.target_pose = target
log.debug('Waiting for Navigation action server...')
self.client.wait_for_server()
log.info('Sending MoveBase goal.')
log.debug('\n' + str(target))
self.base_pending.set()
self.client.send_goal(goal,
feedback_cb=self.base_feedback,
done_cb=self.move_base_done)
def move_base_done(self, status, result):
if self.base_pending.is_set():
if status == GoalStatus.SUCCEEDED:
log.info('Base has reached its destination.')
self.base_pending.clear()
self.dispatcher.emit('move_base.success', result)
elif status in TERMINAL_STATES.keys():
verbose_status = TERMINAL_STATES[status]
log.error('Base has failed to move with %s.', verbose_status)
self.base_pending.clear()
self.dispatcher.emit('move_base.retry', status)
if self.verbose:
log.info('MoveBase goal status: %s', ACTION_STATES[status])
def base_feedback(self, pose):
self.current_pose = pose.base_position
self.dispatcher.emit('move_base.feedback', pose, self.target_pose)
if self.verbose:
log.debug('Current pose updated.')
log.debug(self.current_pose)
def execute_cb(self, goal):
rospy.loginfo("ExecutePathAction received")
# create messages that are used to publish feedback/result
_feedback = ExecutePathFeedback()
_result = ExecutePathResult()
r = rospy.Rate(10)
success = True
aborted = False
goal2 = MoveToAction
#Get obstacle avoidance
self.obstacle_avoidance = rospy.get_param(
'/path_executor/use_obstacle_avoidance')
#Choose server for move_to depending on obstacle avoidance
if self.obstacle_avoidance:
rospy.loginfo("obstacle_avoidance = True")
move_client = SimpleActionClient('/bug2/move_to', MoveToAction)
else:
rospy.loginfo("obstacle_avoidance = False")
move_client = SimpleActionClient('/motion_controller/move_to',
MoveToAction)
move_client.wait_for_server()
#Publish path to /path_executor/current_path
self.publisher.publish(goal.path)
#Iterate through path
for index in range(len(goal.path.poses)):
rospy.loginfo("\nGoal received")
rospy.loginfo("Goal: \n{0}".format(goal.path.poses[index].pose))
#Publish goal pose to move_to server
goal2.target_pose = goal.path.poses[index]
move_client.send_goal(goal2)
rospy.loginfo("Sending goal to move_to action server ...")
while not rospy.is_shutdown():
#Check for preemption
if self._as.is_preempt_requested():
rospy.logwarn('Preempted requested')
move_client.cancel_all_goals()
self._as.set_preempted()
success = False
aborted = True
break
#get result state from move_to server
state = move_client.get_state()
#Success
if state == 3:
rospy.loginfo("Goal reached")
#Publish the feedback
_feedback.pose = goal2.target_pose
_feedback.reached = True
self._as.publish_feedback(_feedback)
#Store to visited poses
_result.visited.append(True)
break
#Unreachable goal
elif state == 4:
rospy.logwarn("Goal is unreachable")
#Skip unreachable
if goal.skip_unreachable:
rospy.logwarn("Skipping unreachable goal")
#Publish the feedback
_feedback.pose = goal2.target_pose
_feedback.reached = False
self._as.publish_feedback(_feedback)
#Store visited poses
_result.visited.append(False)
break
#Abort
else:
rospy.logwarn("Abort unreachable goal")
rospy.loginfo("Cancelling goals...")
move_client.cancel_all_goals()
rospy.logwarn("Goals cancelled")
rospy.loginfo("Aborting action server")
self._as.set_aborted()
rospy.logwarn("Action server aborted")
success = False
aborted = True
break
r.sleep()
if aborted:
break
if success:
self._as.set_succeeded(_result)
rospy.loginfo("Path finished with success\n")
if __name__ == "__main__":
rospy.init_node("nav_to_client")
ac_ = SimpleActionClient("nav_to_node_action", NavToAction)
ac_.wait_for_server()
print 'sever connected!'
help()
while not rospy.is_shutdown():
cmd = raw_input('cmd:')
if cmd == '1':
x = input('target_x:')
y = input('target_y:')
yaw = input('yaw:')
g = NavToActionGoal()
q = tf.transformations.quaternion_from_euler(0, 0, yaw)
g.goal.navgoal.pose.position.x = x
g.goal.navgoal.pose.position.y = y
g.goal.navgoal.pose.orientation.z = q[2]
g.goal.navgoal.pose.orientation.w = q[3]
ac_.send_goal(g.goal,
done_cb=done_cb,
active_cb=active_cb,
feedback_cb=None)
# ac_.wait_for_result()
elif cmd == '2':
ac_.cancel_all_goals()
elif cmd == '3':
break
else:
print 'Invalid Command!'
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 PathPlannerNode(object):
"""
This is a ROS node that is responsible for planning and executing
the a path through the field.
"""
def __init__(self):
# Setup ROS node
rospy.init_node('path_planner')
# ROS params
self.cut_spacing = rospy.get_param("~coverage_spacing", 0.5)
# Setup publishers and subscribers
rospy.Subscriber('heatmap_area', PolygonStamped, self.field_callback)
self.path_marker_pub = rospy.Publisher('visualization_marker',
MarkerArray,
latch=True)
rospy.Subscriber('odom', Odometry, self.odom_callback)
# Setup initial variables
self.field_shape = None
self.field_frame_id = None
self.path = None
self.path_status = None
self.path_markers = None
self.start_path_following = False
self.robot_pose = None
self.goal_state = None
self.current_destination = None
self.testing = False
self.current_distance = None
self.previous_destination = None
self.clear_costmaps = rospy.ServiceProxy('move_base/clear_costmaps',
Empty)
self.just_reset = False
self.timeout = False
# Spin until shutdown or we are ready for path following
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
rate.sleep()
if self.start_path_following:
# Run until stopped
heading = 0
# Setup path following
self.setup_path_following(heading)
# Iterate on path following
while not rospy.is_shutdown():
if not self.step_path_following():
break
self.start_path_following = False
def field_callback(self, msg):
"""
Handles new field polygons, has to be called
at least once before planning happens.
"""
# Convert the PolygonStamped into a shapely polygon
temp_points = []
for point in msg.polygon.points:
temp_points.append((float(point.x), float(point.y)))
self.field_shape = geo.Polygon(temp_points)
self.field_frame_id = msg.header.frame_id
self.start_path_following = True
def odom_callback(self, msg):
"""
Watches for the robot's Odometry data, which is used in the path
planning as the initial robot position.
"""
self.robot_pose = msg
def plan_path(self, field_polygon, origin=None, degrees=0):
"""
This is called after a field polygon has been received.
This uses the automow_planning.coverage module to plan a
coverage path using the field geometry. The path consists of
a series of waypoints.
"""
# Get the rotation to align with the longest edge of the polygon
from automow_planning.maptools import rotation_tf_from_longest_edge, RotationTransform
rotation = rotation_tf_from_longest_edge(field_polygon)
rotation = RotationTransform(rotation.w + degrees)
# Rotate the field polygon
from automow_planning.maptools import rotate_polygon_to
transformed_field_polygon = rotate_polygon_to(field_polygon, rotation)
# Decompose the rotated field into a series of waypoints
from automow_planning.coverage import decompose
print origin
if origin is not None:
point_mat = np.mat([[origin[0], origin[1], 0]],
dtype='float64').transpose()
origin = rotation.irm * point_mat
origin = (origin[0, 0], origin[1, 0])
transformed_path = decompose(transformed_field_polygon,
origin=(origin[0], origin[1]),
width=self.cut_spacing)
# Rotate the transformed path back into the source frame
from automow_planning.maptools import rotate_from
self.path = rotate_from(np.array(transformed_path), rotation)
# Calculate headings and extend the waypoints with them
self.path = self.calculate_headings(self.path)
# Set the path_status to 'not_visited'
self.path_status = []
for waypoint in self.path:
self.path_status.append('not_visited')
# Visualize the data
self.visualize_path(self.path, self.path_status)
def calculate_headings(self, path):
"""
Calculates the headings between paths and adds them to the waypoints.
"""
new_path = []
for index, waypoint in enumerate(path):
new_path.append(list(path[index]))
# If the end, copy the previous heading
if index == 0:
new_path[index].append(0)
continue
# Calculate the angle between this waypoint and the next
dx = path[index][0] - path[index - 1][0]
dy = path[index][1] - path[index - 1][1]
from math import atan2, pi
heading = atan2(dy, dx)
new_path[index].append(heading)
return new_path
def visualize_path(self, path, path_status=None):
"""
Convenience function, calls both visualize_path{as_path, as_marker}
"""
# TODO: finish this (path as path viz)
# self.visualize_path_as_path(path, path_status)
self.visualize_path_as_marker(path, path_status)
def visualize_path_as_path(self, path, path_status=None):
"""
Publishes a nav_msgs/Path msg containing the planned path.
If path_status is not None then the only waypoints put in the
nav_msgs/Path msg will be ones that are 'not_visited' or 'visiting'.
"""
now = rospy.Time.now()
msg = Path()
msg.header.stamp = now
msg.header.frame_id = self.field_frame_id
for index, waypoint in enumerate(path):
# Only put 'not_visited', 'visiting', and the most recent 'visited'
# in the path msg
if path_status != None: # If not set, ignore
if path_status[index] == 'visited': # if this one is visited
try:
# if the next one is visited too
if path_status[index + 1] == 'visited':
# Then continue, because this one doesn't belong
# in the path msg
continue
except KeyError as e: # incase index+1 is too big
pass
# Otherwise if belongs, put it in
pose_msg = PoseStamped()
pose_msg.header.stamp = now
pose_msg.header.frame_id = self.field_frame_id
pose_msg.pose.position.x = waypoint[0]
pose_msg.pose.position.y = waypoint[1]
msg.poses.append(pose_msg)
def visualize_path_as_marker(self, path, path_status):
"""
Publishes visualization Markers to represent the planned path.
Publishes the path as a series of spheres connected by lines.
The color of the spheres is set by the path_status parameter,
which is a list of strings of which the possible values are in
['not_visited', 'visiting', 'visited'].
"""
# Get the time
now = rospy.Time.now()
# If self.path_markers is None, initialize it
if self.path_markers == None:
self.path_markers = MarkerArray()
# # If there are existing markers, delete them
# markers_to_delete = MarkerArray()
# if len(self.path_markers.markers) > 0:
# for marker in self.path_markers.markers:
# marker.action = Marker.DELETE
# markers_to_delete.markers.append(marker)
# self.path_marker_pub.publish(markers_to_delete)
# Clear the path_markers
self.path_markers = MarkerArray()
line_strip_points = []
# Create the waypoint markers
for index, waypoint in enumerate(path):
waypoint_marker = Marker()
waypoint_marker.header.stamp = now
waypoint_marker.header.frame_id = self.field_frame_id
waypoint_marker.ns = "waypoints"
waypoint_marker.id = index
waypoint_marker.type = Marker.ARROW
if index == 0:
waypoint_marker.type = Marker.CUBE
waypoint_marker.action = Marker.MODIFY
waypoint_marker.scale.x = 1
waypoint_marker.scale.y = 1
waypoint_marker.scale.z = 0.25
point = Point(waypoint[0], waypoint[1], 0)
waypoint_marker.pose.position = point
# Store the point for the line_strip marker
line_strip_points.append(point)
# Set the heading of the ARROW
quat = qfe(0, 0, waypoint[2])
waypoint_marker.pose.orientation.x = quat[0]
waypoint_marker.pose.orientation.y = quat[1]
waypoint_marker.pose.orientation.z = quat[2]
waypoint_marker.pose.orientation.w = quat[3]
# Color is based on path_status
status = path_status[index]
if status == 'not_visited':
waypoint_marker.color = ColorRGBA(1, 0, 0, 0.5)
elif status == 'visiting':
waypoint_marker.color = ColorRGBA(0, 1, 0, 0.5)
elif status == 'visited':
waypoint_marker.color = ColorRGBA(0, 0, 1, 0.5)
else:
rospy.err("Invalid path status.")
waypoint_marker.color = ColorRGBA(1, 1, 1, 0.5)
# Put this waypoint Marker in the MarkerArray
self.path_markers.markers.append(waypoint_marker)
# Create the line_strip Marker which connects the waypoints
line_strip = Marker()
line_strip.header.stamp = now
line_strip.header.frame_id = self.field_frame_id
line_strip.ns = "lines"
line_strip.id = 0
line_strip.type = Marker.LINE_STRIP
line_strip.action = Marker.ADD
line_strip.scale.x = 0.1
line_strip.color = ColorRGBA(0, 0, 1, 0.5)
line_strip.points = line_strip_points
self.path_markers.markers.append(line_strip)
# Publish the marker array
self.path_marker_pub.publish(self.path_markers)
def setup_path_following(self, degrees=0):
"""
Sets up the node for following the planned path.
Will wait until the initial robot pose is set and until
the move_base actionlib service is available.
"""
# Create the actionlib service
self.move_base_client = SimpleActionClient('move_base', MoveBaseAction)
connected_to_move_base = False
dur = rospy.Duration(1.0)
# If testing prime the robot_pose
if self.testing:
self.robot_pose = Odometry()
self.robot_pose.pose.pose.position.x = 0
self.robot_pose.pose.pose.position.y = 0
# Wait for the field shape
while self.field_shape == None:
# Check to make sure ROS is ok still
if rospy.is_shutdown(): return
# Print message about the waiting
msg = "Qualification: waiting on the field shape."
rospy.loginfo(msg)
rospy.Rate(1.0).sleep()
# Wait for the robot position
while self.robot_pose == None:
# Check to make sure ROS is ok still
if rospy.is_shutdown(): return
# Print message about the waiting
msg = "Qualification: waiting on initial robot pose."
rospy.loginfo(msg)
rospy.Rate(1.0).sleep()
# Now we should plan a path using the robot's initial pose
origin = (self.robot_pose.pose.pose.position.x,
self.robot_pose.pose.pose.position.y)
self.plan_path(self.field_shape, origin, degrees)
rospy.loginfo("Path Planner: path planning complete.")
# Now wait for move_base
while not connected_to_move_base:
# Wait for the server for a while
connected_to_move_base = self.move_base_client.wait_for_server(dur)
# Check to make sure ROS is ok still
if rospy.is_shutdown(): return
# Update the user on the status of this process
msg = "Path Planner: waiting on move_base."
rospy.loginfo(msg)
# Now we are ready to start feeding move_base waypoints
return
def get_next_waypoint_index(self):
"""
Figures out what the index of the next waypoint is.
Iterates through the path_status's and finds the visiting one,
or the next not_visited one if not visiting on exists.
"""
for index, status in enumerate(self.path_status):
if status == 'visited':
continue
if status == 'visiting':
return index
if status == 'not_visited':
return index
# If I get here then there are no not_visited and we are done.
return None
def distance(self, p1, p2):
"""Returns the distance between two points."""
from math import sqrt
dx = p2.target_pose.pose.position.x - p1.target_pose.pose.position.x
dy = p2.target_pose.pose.position.y - p1.target_pose.pose.position.y
return sqrt(dx**2 + dy**2)
def step_path_following(self):
"""
Steps the path following system, checking if new waypoints
should be sent, if a timeout has occurred, or if path following
needs to be paused.
"""
# Visualize the data
self.visualize_path(self.path, self.path_status)
# Get the next (or current) waypoint
current_waypoint_index = self.get_next_waypoint_index()
# If the index is None, then we are done path planning
if current_waypoint_index == None:
rospy.loginfo("Path Planner: Done.")
return False
if current_waypoint_index == 0:
self.path_status[current_waypoint_index] = 'visited'
# Get the waypoint and status
current_waypoint = self.path[current_waypoint_index]
current_waypoint_status = self.path_status[current_waypoint_index]
# If the status is visited
if current_waypoint_status == 'visited':
# This shouldn't happen...
return True
# If the status is not_visited then we need to push the goal
if current_waypoint_status == 'not_visited':
# Cancel any current goals
#self.move_base_client.cancel_all_goals()
# Set it to visiting
self.path_status[current_waypoint_index] = 'visiting'
# Push the goal to the actionlib server
destination = MoveBaseGoal()
destination.target_pose.header.frame_id = self.field_frame_id
destination.target_pose.header.stamp = rospy.Time.now()
# Set the target location
destination.target_pose.pose.position.x = current_waypoint[0]
destination.target_pose.pose.position.y = current_waypoint[1]
# Calculate the distance
if self.previous_destination == None:
self.current_distance = 5.0
else:
self.current_distance = self.distance(
self.previous_destination, destination)
# Set the heading
quat = qfe(0, 0, current_waypoint[2])
destination.target_pose.pose.orientation.x = quat[0]
destination.target_pose.pose.orientation.y = quat[1]
destination.target_pose.pose.orientation.z = quat[2]
destination.target_pose.pose.orientation.w = quat[3]
# Send the desired destination to the actionlib server
rospy.loginfo("Sending waypoint (%f, %f)@%f" %
tuple(current_waypoint))
self.current_destination = destination
self.move_base_client.send_goal(destination)
self.previous_destination = destination
# If the status is visiting, then we just need to monitor the status
temp_state = self.move_base_client.get_goal_status_text()
if current_waypoint_status == 'visiting':
if temp_state in ['ABORTED', 'SUCCEEDED']:
self.path_status[current_waypoint_index] = 'visited'
else:
duration = rospy.Duration(2.0)
from math import floor
count = 0
self.move_base_client.wait_for_result()
if self.timeout == True:
if count == 6 + int(self.current_distance * 4):
rospy.logwarn(
"Path Planner: move_base goal timeout occurred, clearing costmaps"
)
# Cancel the goals
self.move_base_client.cancel_all_goals()
# Clear the cost maps
self.clear_costmaps()
# Wait 1 second
rospy.Rate(1.0).sleep()
# Then reset the current goal
if not self.just_reset:
self.just_reset = True
self.path_status[
current_waypoint_index] = 'not_visited'
else:
self.just_reset = False
self.path_status[
current_waypoint_index] = 'visited'
return True
self.path_status[current_waypoint_index] = 'visited'
return True
class InterfaceWrapper(object):
def __init__(self, sim=True, move=True):
# rospy.init_node('tests')
# rospy.set_param(DummyInterfaceNodeName + '/initial_beliefstate', 'package://refills_perception_interface/owl/muh.owl')
# rospy.set_param(DummyInterfaceNodeName + '/initial_beliefstate',
# 'package://refills_rooming_in/data/augsburg_rooming_in_map_2018-11-08_10-17-17.owl')
# rospy.set_param(DummyInterfaceNodeName + '/initial_beliefstate',
# 'package://refills_rooming_in/data/rooming_in_map_2018-11-29_16-59-38.owl')
# rospy.set_param(DummyInterfaceNodeName + '/path_to_json',
# 'package://refills_rooming_in/data/augsburg_rooming_in_map_2018-11-08_10-17-17.json')
self.query_shelf_systems_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_shelf_systems', QueryShelfSystems)
self.query_shelf_layers_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_shelf_layers', QueryShelfLayers)
self.query_facings_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_facings', QueryFacings)
self.finish_perception_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/finish_perception', FinishPerception)
self.query_reset_belief_state_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/reset_beliefstate', Trigger)
self.query_shelf_detection_path_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_detect_shelf_layers_path',
QueryDetectShelfLayersPath)
self.query_facing_detection_path_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_detect_facings_path',
QueryDetectFacingsPath)
self.query_product_counting_path_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_count_products_posture',
QueryCountProductsPosture)
self.detect_shelves_ac = SimpleActionClient(
DummyInterfaceNodeName + '/detect_shelf_layers',
DetectShelfLayersAction)
self.detect_facings_ac = SimpleActionClient(
DummyInterfaceNodeName + '/detect_facings', DetectFacingsAction)
self.count_products_ac = SimpleActionClient(
DummyInterfaceNodeName + '/count_products', CountProductsAction)
# self.simple_base_goal_pub = rospy.Publisher('move_base_simple/goal', PoseStamped)
self.simple_joint_goal = rospy.ServiceProxy(
'refills_bot/set_joint_states', SetJointState)
self.sleep = sim
self.sleep_amount = 0
self.move = move
if URDFObject(rospy.get_param(
'robot_description')).get_name() == 'iai_donbot':
from refills_perception_interface.move_arm import MoveArm
self.giskard = MoveArm(avoid_self_collisinon=True)
else:
from refills_perception_interface.move_arm_kmr_iiwa import MoveArm
self.giskard = MoveArm(avoid_self_collisinon=True)
# self.base = MoveBase()
rospy.sleep(.5)
def reset(self):
self.cancel_detect_shelf_layers()
self.cancel_detect_facings()
self.cancel_count_products()
self.query_reset_belief_state()
# -------------------------------------------------------------other------------------------------------------------
def query_shelf_systems(self):
r = self.query_shelf_systems_srv.call(
QueryShelfSystemsRequest()) # type: QueryShelfSystemsResponse
assert len(r.ids) > 0
return r.ids
def finish_perception(self,
expected_error=FinishPerceptionResponse.SUCCESS):
r = self.finish_perception_srv.call(FinishPerceptionRequest())
assert r.error == expected_error
return r
def query_detect_shelf_layers_path(
self,
shelf_id,
expected_error=QueryDetectShelfLayersPathResponse.SUCCESS):
r = self.query_shelf_detection_path_srv.call(
QueryDetectShelfLayersPathRequest(id=shelf_id))
assert r.error == expected_error
# if expected_error == QueryDetectShelfLayersPathResponse.SUCCESS:
# assert len(r.path.postures) > 0
# number_of_joints = -1
# for posture in r.path.postures:
# assert len(posture.joints) > 0
# if number_of_joints == -1: # all postures should have the same number of joints
# number_of_joints = len(posture.joints)
# assert len(posture.joints) == number_of_joints
return r.path
def query_detect_facings_path(
self,
layer_id,
expected_error=QueryDetectFacingsPathResponse.SUCCESS):
r = self.query_facing_detection_path_srv.call(
QueryDetectFacingsPathRequest(id=layer_id))
assert r.error == expected_error
# if expected_error == QueryDetectFacingsPathResponse.SUCCESS:
# assert len(r.path.postures) > 0
# number_of_joints = -1
# for posture in r.path.postures:
# assert len(posture.joints) > 0
# if number_of_joints == -1: # all postures should have the same number of joints
# number_of_joints = len(posture.joints)
# assert len(posture.joints) == number_of_joints
return r.path
def query_count_products_posture(
self,
facing_id,
expected_error=QueryCountProductsPostureResponse.SUCCESS):
"""
:param facing_id:
:param expected_error:
:rtype: FullBodyPosture
"""
r = self.query_product_counting_path_srv.call(
QueryCountProductsPostureRequest(id=facing_id))
assert r.error == expected_error
# if expected_error == QueryCountProductsPostureResponse.SUCCESS:
# assert len(r.posture.joints) > 0
return r.posture
def query_reset_belief_state(self):
assert self.query_reset_belief_state_srv.call(TriggerRequest())
# -------------------------------------------------------layer------------------------------------------------------
def query_shelf_layers(self,
shelf_id,
expected_error=QueryShelfLayersResponse.SUCCESS):
"""
:param shelf_id: str
:rtype: QueryShelfLayersResponse
"""
r = self.query_shelf_layers_srv.call(
QueryShelfLayersRequest(id=shelf_id))
assert r.error == expected_error
return r.ids
def start_detect_shelf_layers(self, shelf_id):
goal = DetectShelfLayersGoal()
goal.id = shelf_id
self.detect_shelves_ac.send_goal(goal)
rospy.sleep(.5)
def get_detect_shelf_layers_result(
self,
expected_state=GoalStatus.SUCCEEDED,
expected_error=DetectShelfLayersResult.SUCCESS):
self.detect_shelves_ac.wait_for_result()
assert self.detect_shelves_ac.get_state() == expected_state
r = self.detect_shelves_ac.get_result()
assert r.error == expected_error
return r
def detect_shelf_layers(self, shelf_system_id, path):
self.start_detect_shelf_layers(shelf_system_id)
if self.sleep:
rospy.sleep(self.sleep_amount)
else:
if path is not None:
self.execute_full_body_path(path)
self.finish_perception()
r = self.get_detect_shelf_layers_result()
assert len(r.ids) > 0
return r.ids
def cancel_detect_shelf_layers(self):
self.detect_shelves_ac.cancel_all_goals()
# ------------------------------------------------------facing------------------------------------------------------
def start_detect_facings(self, layer_id):
goal = DetectFacingsGoal()
goal.id = layer_id
rospy.sleep(.5)
self.detect_facings_ac.send_goal(goal)
rospy.sleep(.5)
def get_detect_facings_result(self,
expected_state=GoalStatus.SUCCEEDED,
expected_error=DetectFacingsResult.SUCCESS):
self.detect_facings_ac.wait_for_result()
assert self.detect_facings_ac.get_state() == expected_state
r = self.detect_facings_ac.get_result()
assert r.error == expected_error
return r
def query_facings(self,
layer_id,
expected_error=QueryFacingsResponse.SUCCESS):
r = self.query_facings_srv.call(QueryFacingsRequest(id=layer_id))
assert r.error == expected_error
return r.ids
def detect_facings(self, layer_id, path):
self.start_detect_facings(layer_id)
if self.sleep:
rospy.sleep(self.sleep_amount)
else:
self.execute_full_body_path(path)
self.finish_perception()
r = self.get_detect_facings_result()
assert len(r.ids) > 0
return r.ids
def cancel_detect_facings(self):
self.detect_facings_ac.cancel_all_goals()
# ------------------------------------------------------counting----------------------------------------------------
def start_count_products(self, facing_id):
goal = CountProductsGoal()
goal.id = facing_id
self.count_products_ac.send_goal(goal)
rospy.sleep(.5)
def get_count_products_result(self,
expected_state=GoalStatus.SUCCEEDED,
expected_error=CountProductsResult.SUCCESS):
self.count_products_ac.wait_for_result()
assert self.count_products_ac.get_state() == expected_state
r = self.count_products_ac.get_result()
assert r.error == expected_error
return r
def count_products(self, facing_id):
if self.move:
rospy.sleep(.5)
self.start_count_products(facing_id)
# self.finish_perception()
r = self.get_count_products_result()
return r.count
def cancel_count_products(self):
self.count_products_ac.cancel_all_goals()
def to_real_joint_name(self, joint_name):
d = {
'Torso_lin1': 'torso_lin_1',
'Torso_lin2': 'torso_lin_2',
'Torso_rot_1': 'torso_rot_1'
}
return d[joint_name]
def execute_full_body_path(self, fbp, sleep=0):
"""
:type fbp: FullBodyPath
"""
if self.move:
for posture in fbp.postures: # type: FullBodyPosture
self.execute_full_body_posture(posture)
rospy.sleep(sleep)
def move_camera_footprint(self, goal_pose):
if self.move:
self.giskard.move_other_frame(goal_pose)
def move_base(self, goal_pose):
if self.move:
self.giskard.move_absolute(goal_pose)
def execute_full_body_posture(self, posture):
"""
:type fbp: FullBodyPath
"""
if self.move:
if posture.type == posture.NO_TYPE:
assert False, '{} has no type'.format(posture)
if posture.type == posture.BASE:
self.move_base(posture.base_pos)
if posture.type == posture.CAMERA or posture.type == posture.BOTH:
self.giskard.set_and_send_cartesian_goal(posture.camera_pos)
if posture.type == posture.JOINT:
self.giskard.set_and_send_joint_goal(posture.goal_joint_state)
if posture.type == posture.CAM_FOOTPRINT or posture.type == posture.BOTH:
self.move_camera_footprint(posture.base_pos)
class ActionCtrl:
BPY_PARAM_SACCADE = "bpy.data.scenes[\"Scene\"].actuators.ACT_saccade.HEAD_PARAM_enabled"
BPY_PARAM_BLINK = "bpy.data.scenes[\"Scene\"].actuators.ACT_blink_randomly.HEAD_PARAM_enabled"
def __init__(self):
# The below will hang until roscore is started!
rospy.loginfo("Starting ghost_bridge_actions node")
rospy.init_node("ghost_bridge_actions", log_level=rospy.DEBUG)
# Publishers for making facial expressions and gestures
self.emotion_state_pub = rospy.Publisher(
"/blender_api/set_emotion_state", EmotionState, queue_size=1)
self.emotion_value_pub = rospy.Publisher(
"/blender_api/set_emotion_value", EmotionState, queue_size=1)
self.gesture_pub = rospy.Publisher("/blender_api/set_gesture",
SetGesture,
queue_size=1)
self.soma_pub = rospy.Publisher("/blender_api/set_soma_state",
SomaState,
queue_size=2)
self.blink_pub = rospy.Publisher("/blender_api/set_blink_randomly",
BlinkCycle,
queue_size=1)
self.saccade_pub = rospy.Publisher("/blender_api/set_saccade",
SaccadeCycle,
queue_size=1)
# Publisher for controlling text to speech, i.e. making text to speech stop
self.robot_name = rospy.get_param("robot_name")
self.tts_control_pub = rospy.Publisher(self.robot_name +
'/tts_control',
String,
queue_size=1)
# Text to speech publisher
self.ghost_tts_pub = rospy.Publisher("/ghost_bridge/say",
GhostSay,
queue_size=1)
# blender set param
self.blender_set_param_srv = rospy.ServiceProxy(
'/blender_api/set_param', SetParam)
# Create gaze action client and wait for server
self.gaze_goal = None
self.gaze_client = SimpleActionClient('/gaze_action', GazeAction)
self.gaze_client.wait_for_server()
# Dictionary of components with controllable parameters. It is structured as
# {component : { paramater : value }}
self.component_parameters = {}
# Speech output parameters based on https://www.w3.org/TR/speech-synthesis/
self.component_parameters["speech"] = {}
self.component_parameters["speech"]["volume"] = 0
self.component_parameters["speech"]["rate"] = 1
# Subscribers to get the available emotions and gestures
rospy.Subscriber("/blender_api/available_emotion_states",
AvailableEmotionStates, self.get_emotions_cb)
rospy.Subscriber("/blender_api/available_gestures", AvailableGestures,
self.get_gestures_cb)
def set_parameter(self, component, parameter, value):
"""Set the parameters of the robot
:param str component: identifier of the robot component or function
:param str parameter: identifier of the parameter
:param float value: the value of the parameter
:return: None
"""
self.component_parameters[component][parameter] = value
rospy.logdebug("Set parameter {}-{} to {}".format(
component, parameter, value))
def say(self, text, fallback_id):
""" Make the robot vocalize text
:param str text: the text to vocalize
:param str fallback_id: the id of the engine to fallback too
:return: None
"""
ssml_template = "<prosody rate=\"{rate}\" volume=\"{volume}dB\"> {} </prosody>"
ssml_text = ssml_template.format(text,
**self.component_parameters["speech"])
msg = GhostSay()
msg.text = ssml_text
msg.fallback_id = fallback_id
self.ghost_tts_pub.publish(msg)
rospy.logdebug("published say(text={}, fallback={})".format(
text, fallback_id))
def say_cancel(self):
""" Stop the robot from vocalizing its current sentence
:return: None
"""
# TODO: the implementation for cancelling robot actions is bloody hacky, we should be using actionlib for
# controlling robot actions. See here: http://wiki.ros.org/actionlib
self.tts_control_pub.publish("shutup")
rospy.logdebug("published shutup")
def gaze_at(self, face_id, speed):
self.gaze_goal = GazeGoal(target=face_id)
self.gaze_client.send_goal(self.gaze_goal, done_cb=self.done_cb)
rospy.logdebug("published gaze_at(face_id={}, speed={})".format(
face_id, speed))
def done_cb(self, msg):
rospy.logdebug("DONE")
def gaze_at_cancel(self):
self.gaze_client.cancel_all_goals()
self.gaze_goal = None
rospy.logdebug("published gaze_at_cancel()")
def blink(self, mean, variation):
""" Set the robot's blink cycle
:param float mean: mean time in seconds between blinks
:param float variation: a random deviation from the mean blink time in seconds
:return: None
"""
msg = BlinkCycle()
msg.mean = mean
msg.variation = variation
self.blink_pub.publish(msg)
rospy.logdebug("published blink(mean={}, variation={})".format(
mean, variation))
def blink_cancel(self):
try:
self.blender_set_param_srv(ActionCtrl.BPY_PARAM_BLINK, "False")
rospy.logdebug(
"blink_cancel: blender_api/set_param service called")
except rospy.ServiceException, e:
rospy.logerr(
"blink_cancel: blender_api/set_param service call failed %s" %
e)
class goto_action(pt.behaviour.Behaviour):
def __init__(self, tag):
rospy.loginfo("Initialising goto action behaviour.")
# parameter
self.tag = tag
## set messages
self.table1_msg = PoseStamped()
#self.table1_msg.header.stamp
self.table1_msg.header.frame_id = 'map'
self.table1_msg.pose.position.x = -1.0
self.table1_msg.pose.position.y = -6.1
self.table1_msg.pose.position.z = 0.0
q = tf.transformations.quaternion_from_euler(0.0, 0.0,
-90.0 * (math.pi / 180))
self.table1_msg.pose.orientation.x = q[0]
self.table1_msg.pose.orientation.y = q[1]
self.table1_msg.pose.orientation.z = q[2]
self.table1_msg.pose.orientation.w = q[3]
self.table2_msg = PoseStamped()
#self.table2_msg.header.stamp
self.table2_msg.header.frame_id = 'map'
self.table2_msg.pose.position.x = 2.6
self.table2_msg.pose.position.y = -1.8
self.table2_msg.pose.position.z = 0.0
q = tf.transformations.quaternion_from_euler(0.0, 0.0,
0.0 * (math.pi / 180))
self.table2_msg.pose.orientation.x = q[0]
self.table2_msg.pose.orientation.y = q[1]
self.table2_msg.pose.orientation.z = q[2]
self.table2_msg.pose.orientation.w = q[3]
# setup action
self.goto_ac = SimpleActionClient('/move_base', MoveBaseAction)
#self.goto_ac.wait_for_server()
# setup service
self.clear_costmaps_srv_nm = '/move_base/clear_costmaps'
rospy.wait_for_service(self.clear_costmaps_srv_nm, timeout=30)
self.clear_costmaps_srv = rospy.ServiceProxy(
self.clear_costmaps_srv_nm, Empty)
# execution checker
self.tried = False
self.done = False
self.start_over_count = 0
# become a behaviour
super(goto_action, self).__init__("goto " + tag + "!")
def update(self):
global cancel_goto_action, moving
if not self.done and cancel_goto_action:
cancel_goto_action = False
self.goto_ac.cancel_all_goals()
self.tried = False
clear_costmaps_req = self.clear_costmaps_srv()
return pt.common.Status.RUNNING
if self.start_over_count < start_over_handler:
self.done = False
self.tried = False
self.start_over_count += 1
if not self.done and not self.tried:
if self.tag == "table1":
goal = MoveBaseGoal()
goal.target_pose = self.table1_msg
goal.target_pose.header.stamp = rospy.Time()
self.goto_ac.send_goal(goal)
elif self.tag == "table2":
goal = MoveBaseGoal()
goal.target_pose = self.table2_msg
goal.target_pose.header.stamp = rospy.Time()
self.goto_ac.send_goal(goal)
self.tried = True
print('###################################################')
return pt.common.Status.RUNNING
# if succesful
if self.done:
moving = False
return pt.common.Status.SUCCESS
else:
moving = True
state = self.goto_ac.get_state()
if state == GoalStatus.SUCCEEDED:
self.done = True
if state == GoalStatus.ABORTED:
self.tried = False
return pt.common.Status.FAILURE
class AggressiveGainBuffNode(object):
def __init__(self, name="aggressive_gain_buff_action"):
self.action_name = name
self._as = SimpleActionServer(self.action_name,
AggressiveGainBuffAction,
execute_cb=self.ExecuteCB,
auto_start=False)
self.gain_buff_state = GainBuffState.ROUTE
self.chassis_state_ = 0
self.sub_odom = rospy.Subscriber("aggressive_buff_info",
AggressiveGainBuffInfo,
callback=self.OdomCB)
self.chassis_mode_client = rospy.ServiceProxy("set_chassis_mode",
ChassisMode)
self.chassis_arrive_state = ChassisArriveState()
self.chassis_mode_ = 0
self._ac_navto = SimpleActionClient("nav_to_node_action", NavToAction)
rospy.loginfo('GAIN_BUFF: Connecting NavTo action server...')
ret = self._ac_navto.wait_for_server()
rospy.loginfo(
'GAIN_BUFF: NavTo sever connected!') if ret else rospy.logerr(
'error: NavTo server not started!')
self.nav_to_error_code = -1
self.search_start_time = rospy.Time(0)
self._ac_look_n_move = SimpleActionClient("look_n_move_node_action",
LookAndMoveAction)
rospy.loginfo('GAIN_BUFF: Connecting Look and Move action server...')
ret = self._ac_look_n_move.wait_for_server(timeout=rospy.Duration(3))
rospy.loginfo('GAIN_BUFF: Look and Move sever connected!'
) if ret else rospy.logerr(
'error: Look and Move server not started!')
self.Look_n_move_error_code = -1
self.thread_ = Thread(target=self.Loop)
self.thread_.start()
self._as.start()
def ExecuteCB(self, goal):
print 'GAIN_BUFF:Aggressive gain buff goal recieved!'
route = goal.route_index
CHASSIS_MODE = ChassisModeForm()
self.nav_to_error_code = -1
while not rospy.is_shutdown():
if self._as.is_preempt_requested():
print 'GAIN_BUFF:PREEMPT REQ'
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
self._ac_navto.cancel_all_goals()
self._ac_look_n_move.cancel_all_goals()
self._as.set_preempted()
return
if self.gain_buff_state == GainBuffState.ROUTE:
if route == 1 or route == 2:
print 'GAIN_BUFF:Route %i received' % route
if route == 1:
self.SetChassisMode(
CHASSIS_MODE.AGGRESSIVE_GAIN_BUFF_ONE)
if route == 2:
self.SetChassisMode(
CHASSIS_MODE.AGGRESSIVE_GAIN_BUFF_TWO)
print 'GAIN_BUFF:Wait for chassis response...'
chassis_wait_start_time = rospy.get_time()
print 'GAIN_BUFF:chassis state is %i' % (
self.chassis_state_)
while self.chassis_state_ == 0 and (
rospy.get_time() -
chassis_wait_start_time) < CHASSIS_MODE_WAIT_TIME:
continue
if self.chassis_state_ == 1:
print 'GAIN_BUFF:Chassis has responded!'
else:
print 'GAIN_BUFF:Chassis does not respond for the new mode! Return.'
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
self._as.set_aborted()
return
chassis_run_start_time = rospy.get_time()
while not (self.chassis_state_
== self.chassis_arrive_state.SUCCEEDED or
(rospy.get_time() - chassis_run_start_time >
CHASSIS_RUN_WAIT_TIME)):
if self.chassis_state_ == self.chassis_arrive_state.SUCCEEDED:
# self._as.set_succeeded()
self.SetChassisMode(
CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
self.gain_buff_state = GainBuffState.SEARCH
self.search_start_time = rospy.get_time()
print 'GAIN_BUFF:Chassis Arrived Buff Area!'
break
else:
print 'GAIN_BUFF:Chassis Aggressive gain buff FAILED, Time out!'
self._as.set_aborted()
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
return
elif route == 3:
print 'GAIN_BUFF:Route 3 received'
self.NavToBuff()
if self.nav_to_error_code == 0:
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
# self._as.set_succeeded()
self.gain_buff_state = GainBuffState.SEARCH
self.search_start_time = rospy.get_time()
print 'GAIN_BUFF:Aggressive Gain Buff Route 3 SUCCEED!'
else:
self._as.set_aborted()
print 'No valied route'
return
elif self.gain_buff_state == GainBuffState.SEARCH:
if (rospy.get_time() -
self.search_start_time) < SEARCH_BUFF_WAIT_TIME:
self.SearchBuff()
else:
print 'SEARCH BUFF: Chassis Aggressive search buff Time Out!'
self._as.set_aborted()
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
self.gain_buff_state = GainBuffState.IDEL
return
else:
print 'Unvaild aggressive gain buff state!'
self._as.set_aborted()
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
return
def Loop(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if self._as.is_preempt_requested():
self._ac_look_n_move.cancel_all_goals()
self._ac_navto.cancel_all_goals()
try:
rate.sleep()
except:
rospy.loginfo('GAIN_BUFF: exiting')
# Set chassis mode to use odom navigation
def SetChassisMode(self, chassis_mode):
if self.chassis_mode_ == chassis_mode:
return
rospy.wait_for_service("set_chassis_mode", timeout=5)
print 'Set chassis mode service connected!'
chassis_mode_msg = ChassisMode()
call_status = self.chassis_mode_client.call(chassis_mode)
chassis_mode_rec_ = ChassisModeResponse()
chassis_mode_rec_ = chassis_mode_msg._response_class
if (call_status and chassis_mode_rec_):
self.chassis_mode_ = chassis_mode
print 'Set Chassis Mode to %i' % chassis_mode
else:
print 'Set gimbal mode failed!'
# return odom arrive result
def OdomCB(self, data):
self.chassis_state_ = data.state
# Call NavTo Action
def NavToBuff(self):
print 'NavTo action is actived!'
g = NavToActionGoal()
for path in BuffPath:
q = tf.transformations.quaternion_from_euler(0, 0, path['yaw'])
g.goal.navgoal.pose.position.x = path['x']
g.goal.navgoal.pose.position.y = path['y']
g.goal.navgoal.pose.orientation.z = q[2]
g.goal.navgoal.pose.orientation.w = q[3]
self._ac_navto.send_goal(g.goal,
done_cb=self.done_cb,
feedback_cb=None)
print 'Waiting for %i point ...' % (path['id'])
self._ac_navto.wait_for_result(timeout=rospy.Duration(10))
if self.nav_to_error_code != 0:
self._ac_navto.cancel_all_goals()
self._as.set_aborted()
print 'AGGRESSIVA_GAIN_BUFF: NavTo Failed!'
return
print 'The result of %i point in BuffPath is arrived!' % path['id']
def SearchBuff(self):
print 'Look and Move action is actived!'
for goal in SearchBuffGoal:
q = tf.transformations.quaternion_from_euler(0., 0., goal['yaw'])
g = LookAndMoveActionGoal()
g.goal.relative_pose.header.frame_id = "map"
g.goal.relative_pose.pose.position.x = goal['x']
g.goal.relative_pose.pose.position.y = goal['y']
g.goal.relative_pose.pose.orientation.z = q[2]
g.goal.relative_pose.pose.orientation.w = q[3]
self._ac_look_n_move.send_goal(g.goal)
self._ac_look_n_move.wait_for_result(timeout=rospy.Duration(10))
if self.Look_n_move_error_code > 0:
self._ac_look_n_move.cancel_all_goals()
self._as.set_aborted()
print 'AGGRESSIVA_GAIN_BUFF: Look and Move Failed!'
return
# Nav_to done result
def done_cb(self, terminal_state, result):
self.nav_to_error_code = result.error_code
class Planner:
def __init__(self, core, datastore, movement_handler):
self.core = core
self.store = datastore
self.movement = movement_handler
rospy.Subscriber('/planned_cmd_vel', Twist,
self.planned_cmd_vel_callback)
rospy.loginfo(
'Initialising a SimpleActionClient for move_base and waiting for connection...'
)
self.sa_client = SimpleActionClient('move_base', MoveBaseAction)
self.sa_client.wait_for_server()
rospy.loginfo('Connected to move_base action server!')
# Aux variables
self.last_goal = None
self.last_planned_cmd_vel_msg = None
self.blacklisted_goals = []
def planned_cmd_vel_callback(self, twist_msg):
self.last_planned_cmd_vel_msg = twist_msg
def find_goal(self):
frontiers_as_indices = self.find_frontiers_as_indices()
if len(frontiers_as_indices) < 1:
return None
# Make sure frontiers are at least one metre apart and NOT in blacklisted regions
f_candidates = []
min_gap = 1.0
for f_ia, f_ib in frontiers_as_indices:
too_close = False
f_x, f_y = self.indices_to_meters(f_ia, f_ib)
# Check that the current frontier is at least `min_gap` away from all other candidate frontiers
for o_ia, o_ib in f_candidates:
o_x, o_y = self.indices_to_meters(o_ia, o_ib)
distance = math.sqrt((f_x - o_x)**2 + (f_y - o_y)**2)
if distance > min_gap:
too_close = True
break
if too_close:
continue
# Check that the current frontier is not in blacklisted regions
blacklisted = False
for bl_x, bl_y, _ in self.blacklisted_goals:
distance = math.sqrt((f_x - bl_x)**2 + (f_y - bl_y)**2)
if distance <= self.core.config['blacklisted_goal_radius']:
blacklisted = True
break
if blacklisted:
continue
# The current frontier is OK, add it to the list of candidates
f_candidates.append((f_ia, f_ib))
# Iterate through all frontiers and find the one that gives maximises information gain
max_eig = -1
best_frontier = None
for f_ia, f_ib in f_candidates:
eig = self.goal_eig(f_ia, f_ib)
if eig > max_eig:
# Check if the goal with best EIG is possible to observer safely
goal_safe = self.find_safe_space_to_observe(f_ia, f_ib)
if goal_safe is None:
continue
max_eig = eig
best_frontier = goal_safe
# Find a safe place to observe the best frontier
if best_frontier is not None:
goal_safe_ia, goal_safe_ib = best_frontier
goal_x, goal_y = self.indices_to_meters(goal_safe_ia, goal_safe_ib)
pos_x, pos_y, yaw = self.store.get_pose()
angle_to_goal = math.atan2(goal_y - pos_y, goal_x - pos_x)
return round(goal_x, 3), round(goal_y, 3), round(angle_to_goal, 3)
# Didn't find anything, return none
return None
def goal_eig(self, ia, ib):
info = self.store.get_map_raw().info
resolution = info.resolution
eig_radius = 3.0
radius = int(eig_radius / resolution)
m, n = self.store.get_map().shape
eig_sum = 0
total = 0
for a in range(ia - radius, ia + radius + 1):
if not in_bounds(a, m):
continue
for b in range(ib - radius, ib + radius + 1):
if not in_bounds(b, n):
continue
distance = math.sqrt((a - ia)**2 + (b - ib)**2)
if distance > radius:
continue
eig_sum += self.cell_eig(a, b)
total += 1
return eig_sum / total
def cell_eig(self, x, y):
p_true = 0.9
p = self.store.get_map()[x, y]
if p < 0:
return 0.5983
elif p == 0 or p >= 1:
return 4.259045e-6
p_o = p_true * p + (1 - p_true) * (1 - p)
p_n = p_true * (1 - p) + (1 - p_true) * p
EH = -p_true * p * np.log(
p_true * p / p_o) - p_true * (1 - p) * np.log(p_true *
(1 - p) / p_n)
return cell_entropy(p) - EH
def find_safe_space_to_observe(self, ia, ib):
costmap = self.store.get_global_costmap()
m, n = costmap.shape
candidates = [
(3, 3),
(3, 0),
(3, -3),
(0, -3),
(-3, -3),
(-3, 0),
(-3, 3),
(0, 3),
]
best_score = -1
best_ia = None
best_ib = None
for c_ia, c_ib in candidates:
new_ia = ia + c_ia
new_ib = ib + c_ib
if not in_bounds(new_ia, m) \
or not in_bounds(new_ib, n) \
or costmap.mask[new_ia, new_ib] \
or costmap[new_ia, new_ib] != 0:
continue
score = 0
for a in range(new_ia - 2, new_ia + 3):
for b in range(new_ib - 2, new_ib + 3):
if not in_bounds(a, m) or not in_bounds(b, n):
continue
if not costmap.mask[a, b] and costmap[a, b] == 0:
score += 1
if score > best_score:
best_score = score
best_ia = new_ia
best_ib = new_ib
if best_ia is not None and best_ib is not None:
return best_ia, best_ib
return None
def find_frontiers_as_indices(self):
costmap = self.store.get_global_costmap()
if costmap is None:
rospy.logwarn(
'No costmap is available - cannot find frontiers as indices, doing nothing.'
)
return []
m, n = costmap.shape
safe_space_indices = np.where(costmap == 0)
frontier_indices = []
candidates = [
(1, -1),
(1, 0),
(1, 1),
(0, 1),
(-1, 1),
(-1, 0),
(-1, -1),
(-1, -1),
(-1, 0),
]
# We define a frontier as a cell that has at at least three free neighbours and and at least three unknown
# neighbours. Remaining neighbours should either be free on unknown.
for (ia, ib) in zip(*safe_space_indices):
# Sanity check - skip masked inputs if they are accidentally included
if costmap.mask[ia, ib]:
continue
free = 0
known_occupied = 0
for off_ia, off_ib in candidates:
ia_new, ib_new = ia + off_ia, ib + off_ib
if 0 <= ia_new < m and 0 <= ib_new < n:
value = costmap[ia_new, ib_new]
masked = costmap.mask[ia_new, ib_new]
if value == 0:
free += 1
elif not masked:
known_occupied += 1
break
else:
# We're add the edge of the costmap, can consider this as an unknown
pass
if known_occupied == 0 and 3 <= free <= 5:
frontier_indices.append((ia, ib))
return frontier_indices
def travel_to_goal(self):
goal_status = self.sa_client.get_state()
if goal_status == GoalStatus.PENDING:
return False
elif goal_status == GoalStatus.ACTIVE:
if self.last_planned_cmd_vel_msg is None:
return False
msg = self.last_planned_cmd_vel_msg
self.movement.request_velocity(lin_x=msg.linear.x,
lin_y=msg.linear.y,
lin_z=msg.linear.z,
ang_x=msg.angular.x,
ang_y=msg.angular.y,
ang_z=msg.angular.z)
return False
elif goal_status in [GoalStatus.RECALLED, GoalStatus.PREEMPTED]:
rospy.logwarn('Goal was recalled or preempted.')
return True
elif goal_status in [GoalStatus.REJECTED, GoalStatus.ABORTED]:
rospy.logwarn(
'Goal was rejected or aborted - this goal was blacklisted.')
self.blacklisted_goals.append(self.last_goal)
self.last_goal = None
return True
elif goal_status == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal succeeded.')
self.last_goal = None
return True
elif goal_status == GoalStatus.LOST:
rospy.loginfo('Goal was lost!')
self.last_goal = None
return True
rospy.logwarn(
'Unrecognised goal status was returned! Assuming goal reaching behaviour terminated.'
)
return True
def publish_goal(self, x_coord, y_coord, yaw):
rospy.loginfo(" requesting to move to {}".format(
(x_coord, y_coord, yaw)))
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = x_coord
goal.target_pose.pose.position.y = y_coord
q = quaternion_from_euler(0, 0, yaw)
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
self.last_goal = (x_coord, y_coord, yaw)
self.sa_client.send_goal(goal)
def cancel_all_goals(self):
self.sa_client.cancel_all_goals()
def indices_to_meters(self, ia, ib):
info = self.store.get_map_raw().info
resolution = info.resolution
x = info.origin.position.x + ib * resolution + resolution / 2
y = info.origin.position.y + ia * resolution + resolution / 2
return x, y
def meters_to_indices(self, x, y):
info = self.store.get_map_raw().info
resolution = info.resolution
ib = int(round((x - info.origin.position.x) / resolution))
ia = int(round((y - info.origin.position.y) / resolution))
return ia, ib
class NavGoal():
def __init__(self):
rospy.Subscriber("nav_goal/goal", MoveBaseGoal, self.goal_callback)
self.done_pub = rospy.Publisher("nav_goal/done", String, queue_size=1)
rospy.Service("nav_goal/resume", Trigger, self.handle_resume)
rospy.Service("nav_goal/cancel", Trigger, self.handle_cancel)
rospy.Service("nav_goal/get_status", Trigger, self.handle_status)
self.grid_move_client = rospy.ServiceProxy("nav_goal/grid_move",
GridMove)
self.clear_map_client = rospy.ServiceProxy("move_base/clear_costmaps",
Empty)
self.ac = SimpleActionClient('move_base', MoveBaseAction)
self.current_goal = MoveBaseGoal()
def goal_callback(self, msg):
rospy.loginfo('recieve a goal')
# clear map
req = EmptyRequest()
self.clear_map_client.call(req)
# call goal service
self.ac.wait_for_server()
self.ac.send_goal(msg,
done_cb=self.done_callback,
active_cb=None,
feedback_cb=None)
self.current_goal = msg # save current goal
rospy.loginfo('send goal')
def done_callback(self, status, result):
rospy.loginfo('goal reached')
# adjust
# qr_req = QRcodeAdjustRequest()
# qr_req.code_message = '2333'
# res = self.qrcode_adjust_client.call(qr_req)
target_pose = PoseStamped()
target_pose.pose.position.x = 0.0
target_pose.pose.position.y = 0.0
target_pose.pose.orientation.w = 0.0
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
# move in
req = GridMoveRequest()
req.dir = 1
req.target_pose = target_pose
res = self.grid_move_client.call(req)
if not res.message == 'success':
rospy.logwarn('get cargo failed')
return None
rospy.loginfo('get cargo success')
def active_callback(self):
rospy.loginfo('goal actived')
def feedback_callback(self):
rospy.loginfo('feedback')
def handle_resume(self, req):
rospy.loginfo('resume current goal')
res = TriggerResponse()
try:
self.goal_callback(self.current_goal) # restart last goal
except Exception:
res.message = 'fail'
return res
res.success = True
res.message = "success"
return res
def handle_cancel(self, req):
rospy.loginfo('cancel current goal')
res = TriggerResponse()
# self.ac.cancel_goal()
self.ac.cancel_all_goals()
res.success = True
res.message = "success"
return res
def handle_status(self, req):
res = TriggerResponse()
try:
state = str(self.ac.get_state())
res.success = True
res.message = state
except Exception:
res.success = False
res.message = 'can not get status'
return res
class GetAmmoNode(object):
_feedback = GetAmmoActionFeedback()
_result = GetAmmoActionResult()
def __init__(self, name="get_ammo_node_action"):
self.action_name = name
self._as = SimpleActionServer(self.action_name,
GetAmmoAction,
execute_cb=self.ExecuteCB,
auto_start=False)
self._as.start()
# initialize gripper
self.gripper = GripperController()
self.gripper.SetState(GripperCmd.NORMAL)
# initialize navto action server
if not SERVO_ONLY and not VISUAL_ONLY:
self._ac_navto = SimpleActionClient("nav_to_node_action",
NavToAction)
rospy.loginfo('GETAMMO: Connecting NavTo action server...')
ret = self._ac_navto.wait_for_server()
rospy.loginfo(
'GETAMMO: NavTo sever connected!') if ret else rospy.logerr(
'error: NavTo server not started!')
if not SERVO_ONLY:
self._ac_lookmove = SimpleActionClient("look_n_move_node_action",
LookAndMoveAction)
rospy.loginfo('GETAMMO: Connecting LookAndMove action server...')
ret = self._ac_lookmove.wait_for_server()
rospy.loginfo('GETAMMO: LookAndMove sever connected!'
) if ret else rospy.logerr(
'error: LookAndMove server not started!')
self.state = GetAmmoStatus.IDLE
self.ammotype = 0
# navigation phase
self.navto_start_time = 0
self.navto_reached = False
self.navto_failed = False
# approach phase
self.servo_cnt = 0
self.servo_no_target = 0
self.servo_base_reached = False
self.servo_top_reached = False
#---
self.looknmove_start_time = 0
self.looknmove_cnt = 0
self.looknmove_no_target = 0
self.looknmove_issued = False
self.looknmove_reached = False
self.looknmove_failed = False
# final phase
self.blind_cnt = 0
self.touch_cnt = 0
self.withdraw_cnt = 0
# process laserscan data
self.sub_top_lidar = rospy.Subscriber("scan2", LaserScan,
self.TopLidarCB)
self.sub_base_lidar = rospy.Subscriber("scan", LaserScan,
self.BaseLidarCB)
self.sub_visual_pose = rospy.Subscriber("visual_pose", PoseStamped,
self.VisualPoseCB)
self.pub_cmd_vel = rospy.Publisher("cmd_vel", Twist, queue_size=1)
self.sub_odom = rospy.Subscriber("odom", Odometry, self.OdomCB)
self.is_stop = False
self.cmd_vel = Twist()
def ExecuteCB(self, goal):
rate = rospy.Rate(20)
rospy.loginfo('GETAMMO: Goal received (%d)' % (goal.ammobox_index))
if SERVO_ONLY:
self.SetStateServo()
if VISUAL_ONLY:
self.SetStateLookMove()
else:
self.state = GetAmmoStatus.MOVETO
id_found = False
box_info = {}
if IAM == "BLUE":
ablist = AmmoBoxes_BLUE
else:
ablist = AmmoBoxes_RED
for box in ablist:
if box['id'] == goal.ammobox_index:
box_info = box
id_found = True
if id_found == False:
rospy.loginfo('GETAMMO: ID %d Not Found, Aborted' %
(goal.ammobox_index))
self._as.set_aborted()
return
checkpoint = box_info.get('checkpoint')[0]
self.ammotype = box_info.get('type')
if checkpoint is None:
rospy.loginfo('GETAMMO: No CheckPoint Info, Aborted')
self._as.set_aborted()
return
g = NavToGoal()
g.navgoal.pose.position.x = checkpoint[0]
g.navgoal.pose.position.y = checkpoint[1]
quat = tf.transformations.quaternion_from_euler(
0., 0., checkpoint[2])
g.navgoal.pose.orientation.z = quat[2]
g.navgoal.pose.orientation.w = quat[3]
self.navto_reached = False
self.navto_failed = False
self.navto_start_time = rospy.get_time()
self._ac_navto.send_goal(g, done_cb=self.NavToDoneCB)
self.gripper.SetState(GripperCmd.NORMAL)
while not rospy.is_shutdown():
# Check Preempt Request
if self._as.is_preempt_requested():
rospy.loginfo('GETAMMO: preempt requested')
self._ac_navto.cancel_all_goals()
self._as.set_preempted()
self.state = GetAmmoStatus.IDLE
break
# Finite State Machine
if self.state == GetAmmoStatus.MOVETO:
if self.navto_reached or (
rospy.get_time() - self.navto_start_time > 15):
self._ac_navto.cancel_all_goals()
if self.ammotype == AmmoType.DOMESTIC_ELEVATED or self.ammotype == AmmoType.ENEMY_ELEVATED:
self.SetStateServo()
else:
self.SetStateLookMove()
elif self.navto_failed:
self._as.set_aborted()
self.state = GetAmmoStatus.IDLE
break
if self.goal.ammobox_index == 26:
self._as.set_succeeded()
self.state = GetAmmoStatus.IDLE
break
elif self.state == GetAmmoStatus.SERVO:
# print self.servo_top_reached, self.servo_base_reached
self.servo_cnt += 1
self.pub_cmd_vel.publish(self.cmd_vel)
if self.servo_no_target > 10:
rospy.loginfo('GETAMMO: Servo no target')
self._as.set_aborted()
self.state = GetAmmoStatus.IDLE
break
if (self.servo_base_reached and self.servo_top_reached):
if NO_GRASP:
self._as.set_succeeded()
self.state = GetAmmoStatus.IDLE
break
self.gripper.SetState(GripperCmd.GRIP_HIGH)
self.blind_cnt = 0
self.state = GetAmmoStatus.BLIND
# SERVO TIME-OUT
if self.servo_cnt > 80:
rospy.logerr('GETAMMO: Servo timeout')
self._as.set_aborted()
self.state = GetAmmoStatus.IDLE
break
elif self.state == GetAmmoStatus.LOOKMOVE:
if rospy.get_time(
) - self.looknmove_start_time > 4 and not self.looknmove_issued:
rospy.loginfo('GETAMMO: Visual timeout')
self._as.set_aborted()
self.state = GetAmmoStatus.IDLE
break
if self.looknmove_cnt == 1:
self.gripper.SetPosition(100, 130)
self.looknmove_cnt += 1
if self.looknmove_no_target > 15:
rospy.loginfo('GETAMMO: Visual no target')
self._as.set_aborted()
self.state = GetAmmoStatus.IDLE
break
if self.looknmove_reached:
if NO_GRASP:
self._as.set_succeeded()
self.state = GetAmmoStatus.IDLE
break
self.blind_cnt = 0
self.gripper.SetState(GripperCmd.GRIP_LOW)
self.state = GetAmmoStatus.VBLIND
self.blind_cnt = 0
# TIME-OUT
if self.looknmove_failed: # or self.visual_cnt > 120:
rospy.logerr('GETAMMO: Look n move timeout.')
self._as.set_aborted()
self.state = GetAmmoStatus.IDLE
break
elif self.state == GetAmmoStatus.BLIND:
self.blind_cnt += 1
if self.gripper.feedback == GripperInfo.TOUCHED:
rospy.loginfo('GETAMMO: Gripper touched.')
self.touch_cnt = 0
self.state = GetAmmoStatus.GRASP
# BLIND APPROACH TIME-OUT
if self.blind_cnt > 80:
self.SendCmdVel(WITHDRAW_VX, 0., 0.)
# rospy.logwarn('BLIND_WITH')
else:
self.SendCmdVel(BLIND_APPROACH_VX, 0., 0.)
# rospy.logwarn('BLIND_APP')
if self.blind_cnt > 100:
# rospy.logwarn('BLIND_EXIT')
rospy.logerr('GETAMMO: Blind approach timeout.')
self._as.set_aborted()
self.state = GetAmmoStatus.IDLE
break
elif self.state == GetAmmoStatus.VBLIND:
self.blind_cnt += 1
if self.gripper.feedback == GripperInfo.TOUCHED:
rospy.loginfo('GETAMMO: Gripper touched.')
self.touch_cnt = 0
self.state = GetAmmoStatus.GRASP
# BLIND APPROACH TIME-OUT
if self.blind_cnt > 80:
self.SendCmdVel(WITHDRAW_VX, 0., 0.)
# rospy.logwarn('BLIND_WITH')
else:
self.SendCmdVel(BLIND_APPROACH_VX * 6, 0., 0.)
# rospy.logwarn('BLIND_APP')
if self.blind_cnt > 100:
# rospy.logwarn('BLIND_EXIT')
rospy.logerr('GETAMMO: Blind approach timeout.')
self._as.set_aborted()
self.state = GetAmmoStatus.IDLE
break
elif self.state == GetAmmoStatus.GRASP:
self.touch_cnt += 1
if self.touch_cnt > 10:
self.SendCmdVel(0., 0., 0.)
else:
if goal.ammobox_index == 5:
self.SendCmdVel(-WITHDRAW_VX * 6, 0., 0.)
elif goal.ammobox_index == 4:
self.SendCmdVel(0., 0., 0.)
else:
self.SendCmdVel(WITHDRAW_VX, 0., 0.)
if self.gripper.feedback == GripperInfo.DONE:
rospy.logerr('GETAMMO: Gripper done.')
self.withdraw_cnt = 0
self.state = GetAmmoStatus.WITHDRAW
# TIMEOUT!
if self.touch_cnt > 200:
self._as.set_aborted()
self.state = GetAmmoStatus.IDLE
break
elif self.state == GetAmmoStatus.WITHDRAW:
self.SendCmdVel(WITHDRAW_VX, 0., 0.)
self.withdraw_cnt += 1
if self.withdraw_cnt > 15:
self._as.set_succeeded()
self.state = GetAmmoStatus.IDLE
break
rate.sleep()
self.gripper.SetState(GripperCmd.NORMAL)
rospy.sleep(.1)
self.SendCmdVel(0., 0., 0.)
rospy.sleep(.1)
self.SendCmdVel(0., 0., 0.)
def TopLidarCB(self, data):
if self.state == GetAmmoStatus.SERVO:
theta = np.deg2rad(
np.linspace(-SERVO_AOV / 2, SERVO_AOV / 2, num=SERVO_AOV + 1))
dist = np.array(data.ranges[179 - SERVO_AOV / 2:179 +
SERVO_AOV / 2 + 1])
range_cut_index = list(np.where(np.abs(dist - 0.5) > 0.25)[0])
theta_cut = np.delete(theta, range_cut_index)
dist_cut = np.delete(dist, range_cut_index)
if theta_cut.size == 0:
self.servo_no_target += 1
# rospy.loginfo('TOP LIDAR: NO TARTGET')
return
y = dist_cut * np.sin(theta_cut)
mean_y = np.average(y)
self.cmd_vel.linear.y = (
TARGET_OFFSET_Y -
mean_y) * KP_VY if self.cmd_vel.angular.z < 0.3 else 0
self.servo_top_reached = np.abs(TARGET_OFFSET_Y - mean_y) < X_ERROR
# print np.abs(TARGET_OFFSET_Y - mean_y)
def BaseLidarCB(self, data):
# angle increment should be 0.5 degree!!!
# print 'base lidar'
if self.state == GetAmmoStatus.SERVO:
theta = np.deg2rad(
np.linspace(-SERVO_AOV_BASE,
SERVO_AOV_BASE,
num=2 * SERVO_AOV_BASE + 1))
dist = np.array(data.ranges[359 - SERVO_AOV_BASE:359 +
SERVO_AOV_BASE + 1])
range_cut_index = list(np.where(np.abs(dist) > 1.0)[0])
theta_cut = np.delete(theta, range_cut_index)
dist_cut = np.delete(dist, range_cut_index)
if theta_cut.size == 0:
rospy.loginfo('BASE LIDAR: NO TARTGET')
return
x = dist_cut * np.cos(theta_cut)
y = dist_cut * np.sin(theta_cut)
mean_x = np.average(x)
# print np.polyfit(x, y, 1)[0]
mean_angle = np.arctan(np.polyfit(x, y, 1)[0])
self.cmd_vel.linear.x = (TARGET_OFFSET_X - mean_x) * KP_VX
self.cmd_vel.angular.z = (TARGET_OFFSET_YAW - mean_angle) * KP_VYAW
self.servo_base_reached = np.abs(
TARGET_OFFSET_X - mean_x) < Y_ERROR and np.abs(
TARGET_OFFSET_YAW - mean_angle) < YAW_ERROR
#print mean_x,mean_angle
# print TARGET_OFFSET_YAW - mean_angle
def VisualPoseCB(self, data):
if self.state == GetAmmoStatus.LOOKMOVE and self.looknmove_issued == False and self.looknmove_cnt > 20:
if data.pose.position.x == 0:
self.looknmove_no_target += 1
# rospy.loginfo('CAMERA: NO TARGET')
else:
err_y = (data.pose.position.z - 300) / 1000
err_x = (data.pose.position.x - 100) / 1000
goal = LookAndMoveGoal()
goal.relative_pose.header.frame_id = "base_link"
goal.relative_pose.pose.position.x = -err_y
goal.relative_pose.pose.position.y = err_x
self._ac_lookmove.cancel_all_goals()
# print goal
rospy.loginfo('GETAMMO: Look n move cmd issued.')
self._ac_lookmove.send_goal(goal,
done_cb=self.LookAndMoveDoneCB)
self.looknmove_issued = True
def NavToDoneCB(self, terminal_state, result):
if terminal_state == GoalStatus.SUCCEEDED:
self.navto_reached = True
self._ac_navto.cancel_all_goals()
rospy.loginfo('GETAMMO: Navto reached')
else:
self.navto_failed = True
rospy.loginfo('GETAMMO: Navto failed')
def LookAndMoveDoneCB(self, terminal_state, result):
print terminal_state, result
if terminal_state == GoalStatus.SUCCEEDED:
self.looknmove_reached = True
self._ac_lookmove.cancel_all_goals()
rospy.loginfo('GETAMMO: LooknMove reached')
else:
self.looknmove_failed = True
rospy.loginfo('GETAMMO: LooknMove failed')
def SendCmdVel(self, vx, vy, vyaw):
self.cmd_vel.linear.x = np.clip(vx, -MAX_LINEAR_VEL, MAX_LINEAR_VEL)
self.cmd_vel.linear.y = np.clip(vy, -MAX_LINEAR_VEL, MAX_LINEAR_VEL)
self.cmd_vel.angular.z = np.clip(vyaw, -MAX_ANGULAR_VEL,
MAX_ANGULAR_VEL)
self.pub_cmd_vel.publish(self.cmd_vel)
# print self.cmd_vel
def SetStateLookMove(self):
self.looknmove_start_time = rospy.get_time()
self.looknmove_cnt = 0
self.looknmove_no_target = 0
self.looknmove_issued = False
self.looknmove_reached = False
self.looknmove_failed = False
self.state = GetAmmoStatus.LOOKMOVE
def SetStateServo(self):
self.servo_cnt = 0
self.servo_no_target = 0
self.servo_base_reached = False
self.servo_top_reached = False
self.state = GetAmmoStatus.SERVO
def OdomCB(self, data):
self.is_stop = data.twist.twist.linear.x < 0.001 and data.twist.twist.linear.y < 0.001
class DockActionServer(ActionServer):
def __init__(self, name):
ActionServer.__init__(self, name, DockAction, self.__goal_callback,
self.__cancel_callback, False)
self.__active = False
self.__docked = False
self.__mutex = threading.Lock()
self.__dock_speed = rospy.get_param('~dock/dock_speed', 0.05)
self.__dock_distance = rospy.get_param('~dock/dock_distance', 1.0)
self.__map_frame = rospy.get_param('~map_frame', 'map')
self.__odom_frame = rospy.get_param('~odom_frame', 'odom')
self.__base_frame = rospy.get_param('~base_frame', 'base_footprint')
self.__dock_ready_pose = Pose()
self.__dock_ready_pose.position.x = rospy.get_param('~dock/pose_x')
self.__dock_ready_pose.position.y = rospy.get_param('~dock/pose_y')
self.__dock_ready_pose.position.z = rospy.get_param('~dock/pose_z')
self.__dock_ready_pose.orientation.x = rospy.get_param(
'~dock/orientation_x')
self.__dock_ready_pose.orientation.y = rospy.get_param(
'~dock/orientation_y')
self.__dock_ready_pose.orientation.z = rospy.get_param(
'~dock/orientation_z')
self.__dock_ready_pose.orientation.w = rospy.get_param(
'~dock/orientation_w')
self.__dock_ready_pose_2 = PoseStamped()
rospy.loginfo('param: dock_spped, %s, dock_distance %s' %
(self.__dock_speed, self.__dock_distance))
rospy.loginfo('param: map_frame %s, odom_frame %s, base_frame %s' %
(self.__map_frame, self.__odom_frame, self.__base_frame))
rospy.loginfo('dock_pose:')
rospy.loginfo(self.__dock_ready_pose)
self.__movebase_client = SimpleActionClient('move_base',
MoveBaseAction)
rospy.loginfo('wait for movebase server...')
self.__movebase_client.wait_for_server()
rospy.loginfo('movebase server connected')
self.__approach_path = Path()
self.__path_tracker = PathTracker()
self.__approach_path_pub = rospy.Publisher('dock_approach_path',
Path,
queue_size=10)
self.__cmd_pub = rospy.Publisher(
'yocs_cmd_vel_mux/input/navigation_cmd', Twist, queue_size=10)
rospy.Subscriber('dock_pose', PoseStamped, self.__dock_pose_callback)
self.__tf_listener = tf.TransformListener()
self.__docked = False
# self.__saved_goal = MoveBaseGoal()
self.__no_goal = True
self.__current_goal_handle = ServerGoalHandle()
self.__exec_condition = threading.Condition()
self.__exec_thread = threading.Thread(None, self.__exec_loop)
self.__exec_thread.start()
rospy.loginfo('Creating ActionServer [%s]\n', name)
def AquireMutex(self):
self.__mutex.acquire(1)
def ReleaseMutex(self):
self.__mutex.release()
def __del__(self):
self.__movebase_client.cancel_all_goals()
def __dock_pose_callback(self, data):
# ps = PoseStamped()
# ps.header.stamp = rospy.Time.now()
# ps.header.frame_id = 'dock'
# ps.pose.position.x = -self.__dock_distance
self.__mutex.acquire(1)
self.__approach_path = Path()
self.__approach_path.header.stamp = rospy.Time.now()
self.__approach_path.header.frame_id = 'odom'
try:
for i in range(6):
p = PoseStamped()
p.header.frame_id = 'dock'
p.pose.position.x = -self.__dock_distance - i * 0.1
# p.pose.orientation = Quaternion(0.0, 0.0, 1.0, 0.0)
p1 = self.__tf_listener.transformPose(self.__odom_frame, p)
self.__approach_path.poses.insert(0, p1)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
# self.__dock_ready_pose_2.pose.position.z = -1.0
rospy.logwarn('tf error, %s' % e)
self.__mutex.release()
self.__approach_path_pub.publish(self.__approach_path)
# rospy.loginfo('path length %lf', len(self.__approach_path.poses))
# try:
# self.__dock_ready_pose_2 = self.__tf_listener.transformPose('map', ps)
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
# self.__dock_ready_pose_2.pose.position.z = -1.0
# rospy.logwarn('tf error, %s' % e)
# self.__dock_ready_pose_2.pose.position.z = 0.0
# rospy.loginfo('get dock pose')
def __goal_callback(self, gh):
rospy.loginfo('get new goal')
if not self.__no_goal:
gh.set_rejected(None, 'robot is busy, rejected')
rospy.logwarn('robot is busy, rejected')
return
self.__exec_condition.acquire()
self.__current_goal_handle = gh
self.__no_goal = False
self.__exec_condition.notify()
self.__exec_condition.release()
def __set_charge_relay(self, state):
pass
def __cancel_callback(self, gh):
self.__active = False
self.__movebase_client.cancel_goal()
rospy.logwarn('cancel callback')
def __get_delta(self, pose, target):
if pose < 0:
pose = math.pi * 2 + pose
if target < 0:
target = math.pi * 2 + target
delta_a = target - pose
delta_b = math.pi * 2 - math.fabs(delta_a)
if delta_a > 0:
delta_b = delta_b * -1.0
if math.fabs(delta_a) < math.fabs(delta_b):
return delta_a
else:
return delta_b
def __rotate(self, delta):
try:
pose, quaternion = self.__tf_listener.lookupTransform(
'odom', self.__base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
(roll, pitch, yaw) = euler_from_quaternion(quaternion)
target_yaw = yaw + delta
if target_yaw > math.pi:
target_yaw = target_yaw - (2.0 * math.pi)
elif target_yaw < -math.pi:
target_yaw = target_yaw + 2.0 * math.pi
rospy.loginfo('rotate %f to %f', delta, target_yaw)
cmd = Twist()
time = rospy.Time.now() + rospy.Duration(15)
while rospy.Time.now() < time and not rospy.is_shutdown(
) and math.fabs(self.__get_delta(yaw, target_yaw)) > 0.03:
try:
pose, quaternion = self.__tf_listener.lookupTransform(
'odom', self.__base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
(roll, pitch, yaw) = euler_from_quaternion(quaternion)
a = self.__get_delta(yaw, target_yaw) * 0.3
if a > 0 and a < 0.3:
cmd.angular.z = 0.3
elif a < 0 and a > -0.3:
cmd.angular.z = -0.3
else:
cmd.angular.z = a
self.__cmd_pub.publish(cmd)
# rospy.loginfo('rotate %f : %f, delta %f, speed %f, %f', target_yaw, yaw, self.__get_delta(yaw, target_yaw), a, cmd.angular.z)
self.__cmd_pub.publish(Twist())
return True
def __head_align(self):
cmd = Twist()
time = rospy.Time.now() + rospy.Duration(10)
while (rospy.Time.now() < time) and self.__active:
try:
dock_pose, dock_quaternion = self.__tf_listener.lookupTransform(
self.__base_frame, 'dock', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
# when dock's x is close to zero, it means dock is just in the front of robot(base_footprint)
if dock_pose[1] < -0.002:
cmd.angular.z = -0.1
elif dock_pose[1] > 0.002:
cmd.angular.z = 0.1
else:
break
rospy.loginfo('algin %f, speed %f', dock_pose[1], cmd.angular.z)
self.__cmd_pub.publish(cmd)
self.__cmd_pub.publish(Twist())
rospy.Rate(0.5).sleep()
return True
def __moveto_dock(self):
cmd = Twist()
cmd.linear.x = -self.__dock_speed
ca_feedback = DockFeedback()
try:
last_pose, last_quaternion = self.__tf_listener.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
delta_distance = 0
while delta_distance < self.__dock_distance - 0.235 and not rospy.is_shutdown(
):
self.__cmd_pub.publish(cmd)
try:
current_pose, current_quaternion = self.__tf_listener.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
delta_distance = math.sqrt(
math.pow(current_pose[0] - last_pose[0], 2) +
math.pow(current_pose[1] - last_pose[1], 2) +
math.pow(current_pose[2] - last_pose[2], 2))
ca_feedback.dock_feedback = 'Moving to Dock, %fm left' % (
self.__dock_distance - delta_distance)
self.__current_goal_handle.publish_feedback(ca_feedback)
# rospy.loginfo('Moving to Dock, %fm left' % (self.__dock_distance-delta_distance))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error aa')
rospy.Rate(20).sleep()
ca_feedback.dock_feedback = 'Stop on Dock'
self.__current_goal_handle.publish_feedback(ca_feedback)
rospy.loginfo('stop robot')
# stop robot
cmd.linear.x = 0
self.__cmd_pub.publish(cmd)
# set charge relay on
self.__set_charge_relay(True)
return True
def __moveto_dock_ready(self):
# step 1
mb_goal = MoveBaseGoal()
mb_goal.target_pose.header.stamp = rospy.Time.now()
mb_goal.target_pose.header.frame_id = self.__map_frame
mb_goal.target_pose.header.seq = 1
mb_goal.target_pose.pose = self.__dock_ready_pose
self.__movebase_client.send_goal(mb_goal)
if self.__movebase_client.wait_for_result():
rospy.loginfo('arrived dock_ready_pose')
return True
rospy.loginfo('unable to move to dock_ready_pose')
# rospy.Rate(2).sleep()
# mb_goal = MoveBaseGoal()
# mb_goal.target_pose.header.seq = 2
# mb_goal.target_pose.header.stamp = rospy.Time.now()
# mb_goal.target_pose.header.frame_id = self.__map_frame
# if self.__dock_ready_pose_2.pose.position.z == -1.0:
# rospy.logwarn('dock_ready_pose_2 failed')
# return False
# else:
# rospy.loginfo('get dock ready pose 2 ()()')
# t = self.__dock_ready_pose_2.pose
# # t.position.z == 0.0
# # t.position.x = -self.__dock_distance
# mb_goal.target_pose.pose = t
# rospy.loginfo('move to dock_ready_pose_2')
# self.__movebase_client.cancel_all_goals()
# self.__movebase_client.send_goal(mb_goal)
# rospy.loginfo(self.__movebase_client.wait_for_result())
# rospy.loginfo('arrived dock_ready_pose_2')
return False
def __dock(self):
if self.__moveto_dock_ready():
if self.__head_align():
# if self.__rotate(math.pi):
# if self.__moveto_dock():
# self.__docked = True
return True
# else:
# rospy.logwarn("unable to move to dock")
# else:
# rospy.logwarn("unable to rotate 180")
else:
rospy.logwarn("unable to align head")
else:
rospy.logwarn("unable to move to dock ready")
self.__docked = False
return False
def __dock_2(self):
# if True:
self.__active = True
if self.__moveto_dock_ready():
c_index = 0
sleep_time = True
# while not rospy.is_shutdown() && self.__active:
while not rospy.is_shutdown():
base_pose = PoseStamped()
base_pose.header.frame_id = 'base_link'
try:
self.__mutex.acquire(1)
if sleep_time:
rospy.sleep(1.0)
sleep_time = False
robot_pose = self.__tf_listener.transformPose(
self.__odom_frame, base_pose)
# rospy.loginfo(len(self.__approach_path.poses))
approach_finish, vel, c_index, get_points = self.__path_tracker.UpdateCmd(
robot_pose, self.__approach_path, c_index)
if get_points == False:
rospy.loginfo('no points')
self.__no_goal = True
self.__mutex.release()
return False
self.__mutex.release()
self.__cmd_pub.publish(vel)
if approach_finish == True:
rospy.loginfo('target reached')
break
# else:
# break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn("except")
# rospy.logwarn(e)
# rospy.logwarn('tf error')
# rospy.logerror('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~')
# break
self.__head_align()
# self.__rotate(3.1)
# self.__moveto_dock()
# self.__docked = True
self.__no_goal = True
if self.__active:
self.__active = False
return True
else:
rospy.logwarn("unable to move to dock ready")
self.__docked = False
return False
def __undock(self):
cmd = Twist()
cmd.linear.x = self.__dock_speed
ca_feedback = DockFeedback()
try:
last_pose, last_quaternion = self.__tf_listener.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error')
delta_distance = 0
while delta_distance < self.__dock_distance - 0.275 and not rospy.is_shutdown(
):
self.__cmd_pub.publish(cmd)
try:
current_pose, current_quaternion = self.__tf_listener.lookupTransform(
self.__odom_frame, self.__base_frame, rospy.Time(0))
delta_distance = math.sqrt(
math.pow(current_pose[0] - last_pose[0], 2) +
math.pow(current_pose[1] - last_pose[1], 2) +
math.pow(current_pose[2] - last_pose[2], 2))
ca_feedback.dock_feedback = 'Undock, %fm left' % (
self.__dock_distance - delta_distance)
self.__current_goal_handle.publish_feedback(ca_feedback)
rospy.loginfo('Undock, %fm left' %
(self.__dock_distance - delta_distance))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logwarn('tf error aa')
rospy.Rate(20).sleep()
ca_feedback.dock_feedback = 'Stop on DockReady'
self.__current_goal_handle.publish_feedback(ca_feedback)
rospy.loginfo('stop robot')
# stop robot
cmd.linear.x = 0.0
self.__cmd_pub.publish(cmd)
# set charge relay off
self.__set_charge_relay(False)
self.__docked = False
self.__current_goal_handle.set_succeeded(None, 'Undocked')
rospy.loginfo('UnDocked')
def __exec_loop(self):
rospy.loginfo('auto dock thread started')
while not rospy.is_shutdown():
with self.__exec_condition:
self.__exec_condition.wait(3)
if self.__no_goal:
continue
rospy.loginfo('processing new goal')
goal = self.__current_goal_handle.get_goal()
# if self.__no_goal:
self.__current_goal_handle.set_accepted('Docking')
state = self.__dock_2()
if state:
self.__current_goal_handle.set_succeeded(None, 'Docked')
rospy.loginfo('Docked')
else:
self.__current_goal_handle.set_aborted(None, 'Dock failed')
rospy.loginfo('Dock failed')
# else:
# self.__current_goal_handle.set_rejected(None, 'already docked')
# if goal.dock == True:
# if self.__docked == True:
# rospy.logwarn('rejected, robot has already docked')
# else:
# rospy.loginfo('Docking')
# self.__current_goal_handle.set_accepted('Docking')
# state = self.__dock_2()
# if self.__docked:
# self.__current_goal_handle.set_succeeded(None, 'Docked')
# rospy.loginfo('Docked')
# else:
# self.__current_goal_handle.set_aborted(None, 'Dock failed')
# rospy.loginfo('Dock failed')
# elif goal.dock == False:
# if self.__docked == False:
# rospy.logwarn('cancel_all_goals')
# self.__movebase_client.cancel_all_goals()
# rospy.logwarn('rejected, robot is not on charging')
# self.__current_goal_handle.set_rejected(None, 'robot is not on charging')
# else:
# rospy.loginfo('Start undock')
# self.__current_goal_handle.set_accepted('Start undock')
# self.__undock()
# else:
# rospy.logwarn('unknown dock data type, should be true or false')
# self.__current_goal_handle.set_succeeded(None, 'Docked')
rospy.loginfo('new goal finish')
self.__no_goal = True
rospy.loginfo('auto dock thread stop')
class PR2JointTrajectoryControllerManager(object):
def __init__(self, namespace):
"""
Setup connection to ``namespace/joint_trajectory_action`` action server
and ``namespace/state`` state topic.
:param str namespace: namespace for the controller from where
the action server and the state topic hang, e.g. /head_controller
/head_controller/follow_joint_trajectory/[goal/feedback/cancel/result]
/head_controller/state
"""
self.ns = namespace
self.goal_done = False
self.last_state = None
self.state_topic = self.ns + '/state'
self._state_sub = rospy.Subscriber(
self.state_topic,
JointTrajectoryControllerState,
self.state_cb,
# Only the last state is important
queue_size=1)
self.as_name = self.ns + '/follow_joint_trajectory'
self._ac = SimpleActionClient(self.as_name,
FollowJointTrajectoryAction)
self.connect_as()
self.wait_for_state()
self.managed_joints = self.last_state.joint_names
def connect_as(self):
"""
Connect to the action server printing warnings while
it's waiting.
"""
rospy.loginfo("JointTrajectoryControllerManager: connecting to AS '" +
str(self.as_name) + "'")
while not rospy.is_shutdown() and not self._ac.wait_for_server(
rospy.Duration(5.0)):
rospy.logwarn("Waiting for AS '" + self.as_name + "'...")
def wait_for_state(self):
"""
Wait for the first state message.
"""
rospy.loginfo("Waiting for first state on: '" + self.state_topic +
"'...")
while not rospy.is_shutdown() and self.last_state is None:
rospy.sleep(2.0)
rospy.logwarn("Waiting for first state on: '" + self.ns +
"/state'...")
def state_cb(self, state):
"""
Save last state received.
:param JointTrajectoryControllerState state: last state.
"""
self.last_state = state
def send_goal(self, goal):
"""
Send goal to the controller.
:param JointTrajectory goal: Goal to send.
"""
self.goal_done = False
self.last_result = None
self.last_feedback = None
# Check which of our joints the goal has
joints_in_goal = []
for j in goal.joint_names:
if j in self.managed_joints:
joints_in_goal.append(j)
rospy.loginfo("This controller manages the joints: " +
str(self.managed_joints))
rospy.loginfo("Goal contains our joints: " + str(joints_in_goal))
missing_joints = list(set(self.managed_joints) - set(joints_in_goal))
rospy.loginfo("Goal is missing joints: " + str(missing_joints))
extra_joints = list(set(goal.joint_names) - set(joints_in_goal))
rospy.loginfo("Discarding joints not from this controller: " +
str(extra_joints))
if len(extra_joints) > 0:
# Create a new goal only with the found joints, in the next
# step it will be filled with any missing joints
jt = JointTrajectory()
jt.joint_names = joints_in_goal
for jp in goal.points:
p = JointTrajectoryPoint()
p.time_from_start = jp.time_from_start
for jname in joints_in_goal:
idx = goal.joint_names.index(jname)
p.positions.append(jp.positions[idx])
if jp.velocities:
p.velocities.append(jp.velocities[idx])
if jp.accelerations:
p.accelerations.append(jp.accelerations[idx])
if jp.effort:
p.effort.append(jp.effort[idx])
jt.points.append(p)
goal = jt
if len(missing_joints) == len(self.managed_joints):
# This goal contains no joints of this controller
# we don't do anything
rospy.loginfo("Goal contains no joints of this controller.")
return
if len(missing_joints) > 0:
# Fill message for the real controller with the missing joints
# Transform tuple fields into lists
goal.joint_names = list(goal.joint_names)
for point in goal.points: # type: JointTrajectoryPoint
point.positions = list(point.positions)
if point.velocities:
point.velocities = list(point.velocities)
if point.accelerations:
point.accelerations = list(point.accelerations)
if point.effort:
point.effort = list(point.effort)
# Now add the missing joints
for jname in missing_joints:
goal.joint_names.append(jname)
jidx = self.managed_joints.index(jname)
# TODO: remove this if-assert (it's redundant)
if jidx == -1:
rospy.logerror("Couldn't find joint " + jname +
", which we should be controlling," +
" in our managed joints.")
assert False
for point in goal.points:
point.positions.append(
self.last_state.actual.positions[jidx])
if point.velocities:
point.velocities.append(
self.last_state.actual.velocities[jidx])
if self.last_state.actual.accelerations:
point.accelerations.append(
self.last_state.actual.accelerations[jidx])
# TODO: deal with forces, they aren't published in state...
# but they are in joint_states
# Now the trajectory should be complete and we can send the goal
jtg = FollowJointTrajectoryGoal()
jtg.trajectory = goal
rospy.loginfo("Sending goal: " + str(goal))
self._ac.send_goal(jtg,
done_cb=self.done_cb,
feedback_cb=self.feedback_cb)
def get_managed_joints(self):
"""
Convenience method, return list of managed joints.
:returns list: list of strings of managed joints.
"""
return self.managed_joints
def is_goal_done(self):
"""
Convenience method, return true if the last goal is done.
:returns bool: True if goal done, False otherwise.
"""
return self.goal_done
def get_result(self):
"""
Convenience method, returns result.
:returns FollowJointTrajectoryResult: result.
"""
return self.last_result
def done_cb(self, state, result):
"""
:param int state: int as state actionlib_msgs/GoalStatus.
:param FollowJointTrajectoryResult result: result of the action server.
"""
self.goal_done = True
self.last_result = result
def get_feedback(self):
"""
Convenience method, get feedback.
:returns FollowJointTrajectoryFeedback: feedback.
"""
return self.last_feedback
def feedback_cb(self, feedback):
"""
:param FollowJointTrajectoryFeedback feedback: feedback message.
"""
self.last_feedback = feedback
def cancel_all_goals(self):
"""
Cancel all current goals if any.
"""
self._ac.cancel_all_goals()
class MoveArm(object):
# Variablen die den Schnitt unmittelbar beeinflussen
offset_tip = 0.102 # Offset in cm (Dicke des Schneidbretts + Abstand zw. Fingerspitze und Klingenunterseite)
blade_len = 0.05 # Laenge der Klinge
ft_limit = 50 # Kraft Grenzwert
ft_threshold = 25 # Kraft Schwellwert
step_down = 0.01 # Standard Schnitttiefe
def __init__(self, enabled=True):
self.enabled = enabled
self.client = SimpleActionClient('/qp_controller/command',
ControllerListAction)
rospy.loginfo('connecting to giskard')
self.client.wait_for_server()
rospy.loginfo('connected to giskard')
self.tip = 'gripper_tool_frame'
self.root = 'base_link'
self.joint_names = [
'shoulder_pan_joint',
'shoulder_lift_joint',
'elbow_joint',
'wrist_1_joint',
'wrist_2_joint',
'wrist_3_joint',
]
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
# Subcriber fuer das Topic"/kms40/wrench_zeroed". Wenn Nachrichten empfangen werden, wird die Funktion
# ft_callback aufgerufen.
self.ft_sub = rospy.Subscriber("/kms40/wrench_zeroed", WrenchStamped,
self.ft_callback)
self.ft_list = [] #Liste fuer die gemessenen Kraftwerte.
# Service, um den Offset des Kraftmomentensensors zu aktualisieren.
# Der Service wartet auf ein Objekt vom Typ Trigger.
self.reset_ft = rospy.ServiceProxy("/ft_cleaner/update_offset",
Trigger)
rospy.sleep(1)
self.reset_ft.call(TriggerRequest()) #Trigger Objekt
# Publisher fuer die Position des Endeffektors
self.endeffector_pub = rospy.Publisher('endeffector_position',
PoseStamped)
def send_cart_goal(self,
goal_pose,
translation_weight=1,
rotation_weight=1):
if self.enabled:
goal = ControllerListGoal()
goal.type = ControllerListGoal.STANDARD_CONTROLLER
# translation
controller = Controller()
controller.type = Controller.TRANSLATION_3D
controller.tip_link = self.tip
controller.root_link = self.root
controller.goal_pose = goal_pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.05
controller.weight = translation_weight
goal.controllers.append(controller)
# rotation
controller = Controller()
controller.type = Controller.ROTATION_3D
controller.tip_link = self.tip
controller.root_link = self.root
controller.goal_pose = goal_pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.2
controller.weight = rotation_weight
goal.controllers.append(controller)
self.client.send_goal(goal)
result = self.client.wait_for_result(rospy.Duration(10))
print('finished in 10s?: {}'.format(result))
def relative_goal(self,
position,
orientation,
translation_weight=1,
rotation_weight=1):
p = PoseStamped()
p.header.frame_id = self.tip
p.pose.position = Point(*position)
p.pose.orientation = Quaternion(*orientation)
self.send_cart_goal(p, translation_weight, rotation_weight)
def send_joint_goal(self, joint_state):
if self.enabled:
goal = ControllerListGoal()
goal.type = ControllerListGoal.STANDARD_CONTROLLER
# translation
controller = Controller()
controller.type = Controller.JOINT
controller.tip_link = self.tip
controller.root_link = self.root
controller.goal_state = joint_state
controller.p_gain = 3
controller.enable_error_threshold = False
controller.threshold_value = 0.01
controller.weight = 1.0
goal.controllers.append(controller)
self.client.send_goal(goal)
result = self.client.wait_for_result(rospy.Duration(10))
print('finished in 10s?: {}'.format(result))
def ft_callback(self, data):
"""
Callback fuer den Kraft-/Momentensensor
:param data: Sensordaten
:type: WrenchStamped
"""
# Abbruch der Bewegung, wenn gemessene Kraft das Limit ueberschreitet, um Sicherheitsabschaltung des Arms zuvorzukommen.
if abs(data.wrench.force.z) > self.ft_limit:
print("Stop")
self.client.cancel_all_goals()
# Lokalisation des Endeffektors
trans = self.tfBuffer.lookup_transform('arm_mounting_plate', self.tip,
rospy.Time())
p = PoseStamped()
p.header = trans.header
p.pose.position.x = trans.transform.translation.x
p.pose.position.y = trans.transform.translation.y
p.pose.position.z = trans.transform.translation.z
p.pose.orientation.x = trans.transform.rotation.x
p.pose.orientation.y = trans.transform.rotation.y
p.pose.orientation.z = trans.transform.rotation.z
p.pose.orientation.w = trans.transform.rotation.w
self.endeffector_pub.publish(p)
# Der absolute Kraftwert wird ausgelesen und zu einer Liste hinzugefuegt, die die Werte aneinander reiht, um
# spaeter daraus eine Schneidestrategie abzuleiten.
ft = abs(data.wrench.force.z)
self.ft_list.append(ft)
def move_tip_in_amp(self, x, y, z):
"""
Bewegung des Gripper Tool Frame in Bezug auf den Frame 'arm_mounting_plate' (amp).
:type x,y,z: Distanz in Meter
:param x: Bewegung entlang der x-Achse des Frame 'arm_mounting_plate'
:param y: Bewegung entlang der y-Achse des Frame 'arm_mounting_plate'
:param z: Bewegung entlang der z-Achse des Frame 'arm_mounting_plate'
"""
# Ermittelung der Position des Frames Gripper Tool in Bezug auf 'arm_mounting_plate'-Frame
trans = self.tfBuffer.lookup_transform('arm_mounting_plate', self.tip,
rospy.Time())
p = PoseStamped()
p.header.frame_id = 'arm_mounting_plate'
# Die soeben ermittelten Werte werden uebernommen und mit den Werten der gewuenschten Bewegung addiert.
p.pose.position = trans.transform.translation
p.pose.position.x += x
p.pose.position.y += y
p.pose.position.z += z
p.pose.orientation = trans.transform.rotation
cut.send_cart_goal(p)
def distance2table(self):
"""
Berechnung des Abstandes von Klingenunterkante zu Oberflaeche der Schneidunterlage.
:rtype: Distanz in Meter
:return: Abstand zum Tisch
"""
# Abfrage der Position des Frames 'gripper_tool_frame' in Bezug auf 'arm_mounting_plate'.
# Das Frame 'arm_mounting_plate' entspricht dabei der Tischoberkante.
trans = self.tfBuffer.lookup_transform('arm_mounting_plate', self.tip,
rospy.Time())
# Kalkulation des Abstandes von Klingen-Unterseite zum Schneidebrett mit Offset.
distance2table = trans.transform.translation.z - self.offset_tip
return distance2table
def go_to_home(self):
"""
Definition der Standard Position.
:return:
"""
print("Approach Home Pose")
goal_joint_state = JointState()
goal_joint_state.name = self.joint_names
goal_joint_state.position = [
-2.417572323475973, -1.530511204396383, -1.6327641646014612,
-1.5507991949664515, 1.5708668231964111, 1.509663701057434
]
self.send_joint_goal(goal_joint_state)
print("Home Pose Approached")
def align(self):
"""
Ausrichtung des Messers. Das Messer wird nach vorne gekippt, da die Klinge gebogen ist und sonst nicht buendig
mit dem Schneidebrett abschliessen wuerde. Der tiefste Punkt der Klinge befindet sich so zentral ueber dem Objekt.
Ebenfalls wird das Messer um die z-Achse gedreht, da die provisorische Halterung das Messer nicht perfekt
ausgerichtet aufnimmt. Diese Werte sind bei Verwendung von anderem Messer und Halterung anzupassen.
"""
q = quaternion_from_euler(0, -0.15, 0.02, 'ryxz')
cut.relative_goal([0, 0, 0], q)
cut.move_tip_in_amp(-0.01, 0, 0)
# Weitere Bewegungen des Endeffektors, die nicht beruecksichtigt wurden.
# Einfache Schnittbewegung entlang der y-Achse (in Bezug auf gripper_tool_frame) bei gleicher Orientierung des Grippers
# def straight_cut(self):
# d2t = test.distance2table()
# test.relative_goal([0,-d2t,0],[0,0,0,1])
#
# Hackende Bewegung
# def straight_chop(self):
# max_step = 6
# for i in range(max_step):
# test.relative_goal([0,-0.02,0],[0,0,0,1])
# test.relative_goal([0,0.01,0], [0, 0, 0, 1])
#
# Saegende Bewegung
# def saw(self):
# max_step = 6
# for i in range(max_step):
# test.relative_goal([0, -0.005, 0.05], [0, 0, 0, 1])
# test.relative_goal([0, -0.005, -0.05], [0, 0, 0, 1])
#
# Einfache rollende Schnittbewegung
# def roll_simple(self):
# q = quaternion_from_euler(0, 0.3, 0, 'ryxz')
# test.relative_goal([0,0,0],q,translation_weight=100) # Erhohung der Gewichtung der Translation, damit die Spitze genauer in Position bleibt
# test.move_tip_in_amp(0, 0, -0.08)
# q_1 = quaternion_from_euler(0, -0.3, 0, 'ryxz')
# test.relative_goal([0, 0, 0],q_1,translation_weight=100)
#
# Erweiterte rollende Schnittbewegung
# def roll_advanced(self):
# q = quaternion_from_euler(0, 0.3, 0, 'ryxz')
# test.relative_goal([0, 0, 0], q, translation_weight=100)
# test.move_tip_in_amp(0, 0, -0.08)
# test.move_tip_in_amp(-0.05, 0, 0)
# q_1 = quaternion_from_euler(0, -0.3, 0, 'ryxz')
# test.relative_goal([0, 0, 0], q_1, translation_weight=100)
#
#
# def cross_cut(self):
# max_step = 5
# for i in range(max_step):
# q = quaternion_from_euler(0, 0.1, 0, 'ryxz')
# test.relative_goal([0, 0, 0], q, translation_weight=100)
# test.relative_goal([0, -0.01, 0.05], [0, 0, 0, 1])
# q_1 = quaternion_from_euler(0, -0.1, 0, 'ryxz')
# test.relative_goal([0, 0, 0], q_1,translation_weight=100)
# test.relative_goal([0, 0, -0.05], [0, 0, 0, 1])
def master_cut(self):
"""
Funktion fuer die Planung und Ausfuehrung des Schnitte
:return:
"""
# Abfrage des aktuellen Abstands von Klinge zu Schneidebrett.
d2t = cut.distance2table()
# Solange dieser Abstand positiv ist, also sich das Messer oberhalb des Schneidbretts befindet, wird geschnitten.
while d2t > 0:
# Aufruf der Funktion, die die Bewegung unter Beruecksichtig verschiedener Paramenter berechnet und zurueckgibt.
down, side, final = cut.calc_move()
# Bewegung, wenn der gemessene F/T Wert den Schwellwert nicht ueberschritten hat. In diesem Fall erfolgt die
# Bewegung rein entlang der z-Achse.
if side == 0:
cut.move_tip_in_amp(0, 0, -down)
# Wenn F/T-Wert den Grenzwert ueberschreitet, kommt eine Bewegung in x Richtung dazu.
# Dabei wird zunaechst die Klinge ohne Bewegung zurueck gefahren, um von der vollen Klingenlaenge
# zu profitieren. Anschliessend erfolgt eine diagonale Schnittbewegung ueber die gesamte Klingenlaenge.
# Abschliessend eine weitere diagonale Bewegung, um wieder in die Ausgangsposition (x-Achse) zu gelangen.
else:
cut.move_tip_in_amp(-side, 0, -(1 / 4) * down)
cut.move_tip_in_amp(2 * side, 0, -(2 / 4) * down)
cut.move_tip_in_amp(-side, 0, -(1 / 4) * down)
# Wenn die letze Bewegung ausgefuehrte wurde (also Final == True von calc_move() zurueckgegeben wird),
# wird die Funktion beendet. Der Schnittvorgang wird mit Bewegungen entlag der x-Achse abgeschlossen,
# um sicherzustellen, dass das Objekt in Gaenze durchtrennt wird. Abschliessend faehrt das Messer seitlich
# entlang der y-Achse um das abgetrennte Stueck zu separieren.
if final == True:
print("Final")
cut.move_tip_in_amp(-self.blade_len, 0, 0)
cut.move_tip_in_amp(self.blade_len * 1.5, 0, 0)
cut.move_tip_in_amp(-self.blade_len / 2, 0, 0.005)
cut.move_tip_in_amp(0, 0.05, 0)
print("Cut Finished")
return
# Funktion um die Schnittbewegung zu berechnen
def calc_move(self):
"""
Return of three values necessary for cut-move execution.
:return: 1.value:cutting depth; 2.value: lateral move; 3.value: final cut
:type: (float,float,bool)
"""
# Init
final = False
# Abfrage des maximalen F/T-Werts aus der letzten Bewegung
cur_ft = cut.max_ft()
# cur_ft = self.ft_threshold
# Abfrage des aktuellen Abstands von Klingenunterseite zu Schneidebrett
d2t = cut.distance2table()
print("Distance to Table %s" % d2t)
print("Current FT %s" % cur_ft)
# Wenn der Kraftwert den Schwellwert unterschreitet, wird nur entlang der z-Achse geschnitten
if cur_ft < self.ft_threshold:
down = self.step_down
side = 0
# Wenn die Schritttiefe kleiner als der Abstand zur Oberflaeche ist,
# wird die Schritttiefe der naechsten Bewegung auf die verbleibende Distanz gesetzt,
# um Kollisionen mit dem Tisch zu vermeiden.
if d2t <= down:
down = d2t
final = True
else:
# Berechnung der Bewegung, wenn der Kraftschwellwert ueberschritten wird.
# Je hoeher der gemessene Kraftwert, desto geringer die Schnitttiefe.
# Die maximale berechnete Schnitttiefe entspricht der Standardschnitttiefe,
# wenn die Kraft dem Schwellwert entspricht.
down = (self.ft_threshold / cur_ft) * (self.step_down)
# Setzen einer Mindestschnitttiefe
if down < 0.001:
down = 0.001
# Wenn die berechnete Schrittweite kleiner als der Abstand zur Oberflaeche,
# wird die Schrittweite der naechsten Bewegung entsprechend angepasst,
# um Kollisionen mit dem Tisch zu vermeiden.
if d2t <= down:
down = d2t
final = True # Letzte Bewegung
# Die seitliche Bewegung entspricht der Haelfte der Klingenlaenge.
side = self.blade_len / 2
print("Side %s" % side)
print("Down %s" % down)
return (down, side, final)
def max_ft(self):
"""
Funktion um den maximalen F/T waehrend der vergangenen Bewegung auszulesen und zurueckzugeben.
:return: Maximale Kraft waehrend der letzten Bewegung
"""
# Abfrage des max. F/T aus der Liste der F/T Werte
ft_max = max(self.ft_list)
# Nach dem der Wert zwischengespeichert worden ist, wird die Liste fuer den naechsten Durchlauf geleert.
self.ft_list = []
print("Max. FT: %s" % ft_max)
return (ft_max)
class SafeLand(object):
def __init__(self, height, safe_dist):
self.quadrotor_height = height # Height of the motion to the ground
self.safe_dist = safe_dist # Distance the drone must maintain from a victim
# Create safe_land server
self.land_server = SimpleActionServer('safe_land_server',
ExecuteLandAction,
self.land_Callback, False)
self.server_feedback = ExecuteLandFeedback()
self.server_result = ExecuteLandResult()
# Get client from trajectory server
self.trajectory_client = SimpleActionClient(
"approach_server", ExecuteDroneApproachAction)
self.trajectory_client.wait_for_server()
self.point_to_go = ExecuteDroneApproachGoal(
) # Message to define next position to look for victims
# quadrotor motion service
self.motors = rospy.ServiceProxy('enable_motors', EnableMotors)
self.motors.wait_for_service()
## variables
self.sonar_me = Condition()
self.odometry_me = Condition()
# Start trajectory server
self.land_server.start()
def land_Callback(self, msg):
'''
Execute a the landing of the drone by avoiding to land above victims
'''
self.current_height = None
self.odometry = None
# Subscribe to sonar_height
sonar_sub = rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
# Subscribe to ground_truth to monitor the current pose of the robot
odom_sub = rospy.Subscriber("ground_truth/state", Odometry,
self.poseCallback)
# Subscribe to victim sensor
sensor_sub = rospy.Subscriber("victim_sensor/out",
events_message,
self.v_sensor_callback,
queue_size=10)
self.sonar_me.acquire()
if not self.current_height:
self.sonar_me.wait()
self.odometry_me.acquire()
if not self.odometry:
self.odometry_me.wait()
self.point_to_go.goal.position.x = self.odometry.position.x
self.point_to_go.goal.position.y = self.odometry.position.y
self.point_to_go.goal.position.z = self.odometry.position.z - self.current_height + self.quadrotor_height
self.point_to_go.goal.orientation.x = self.odometry.orientation.x
self.point_to_go.goal.orientation.y = self.odometry.orientation.y
self.point_to_go.goal.orientation.z = self.odometry.orientation.z
self.point_to_go.goal.orientation.w = self.odometry.orientation.w
self.odometry_me.release()
self.sonar_me.release()
state = None
while not (state in [GoalStatus.SUCCEEDED, GoalStatus.ABORTED]):
# print(self.point_to_go)
if self.land_server.is_preempt_requested():
self.land_server.set_preempted(self.server_result)
return
self.trajectory_client.send_goal(self.point_to_go)
self.trajectory_client.wait_for_result() # Wait for the result
state = self.trajectory_client.get_state(
) # Get the state of the action
if state == GoalStatus.SUCCEEDED:
self.sonar_me.acquire()
self.odometry_me.acquire()
if self.current_height > (self.quadrotor_height * 1.20):
if not self.current_height:
self.sonar_me.wait()
if not self.odometry:
self.odometry_me.wait()
# quadrotor is too high from ground
self.point_to_go.goal.position.z = self.odometry.position.z - self.current_height + self.quadrotor_height
state = None
else:
#Safe Land soccesfully executed
motors_msg = EnableMotorsRequest()
motors_msg.enable = False # Disable motors to make the drone land
self.motors.call(motors_msg)
# Send landed pose as result
self.odometry_me.wait()
self.server_result.land_pose = self.odometry
self.land_server.set_succeeded(self.server_result)
self.odometry_me.release()
self.sonar_me.release()
elif state == GoalStatus.ABORTED:
self.land_server.set_aborted()
sonar_sub.unregister()
odom_sub.unregister()
sensor_sub.unregister()
# rospy.logwarn("ENDING safe land!")
return
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
if self.current_height < self.quadrotor_height * 0.8:
self.odometry_me.acquire()
if not self.odometry:
self.odometry_me.wait()
self.point_to_go.goal.position.z = self.odometry.position.z - self.current_height + self.quadrotor_height
self.odometry_me.release()
self.trajectory_client.cancel_goal()
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
self.odometry_me.notify_all()
self.odometry_me.release()
def v_sensor_callback(self, msg):
'''
Receive position of a detected victim
'''
self.sonar_me.acquire()
actual_x = self.odometry.position.x
actual_y = self.odometry.position.y
self.sonar_me.release()
victim_x = msg.position[0].linear.x
victim_y = msg.position[0].linear.y
dist = sqrt((actual_x - victim_x)**2 + (actual_y - victim_y)**2)
if dist < self.safe_dist:
self.point_to_go.goal.position.x = victim_x + (
self.safe_dist / dist) * (actual_x - victim_x)
self.point_to_go.goal.position.y = victim_y + (
self.safe_dist / dist) * (actual_y - victim_y)
rospy.logwarn("Replannig safe land!!!!")
self.trajectory_client.cancel_all_goals()
