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()