def cb(self, move_base_goal): move_base_goal = self.tf.transformPose('map', move_base_goal.target_pose) goal_js = [ move_base_goal.pose.position.x, move_base_goal.pose.position.y, rotation_from_matrix( quaternion_matrix([ move_base_goal.pose.orientation.x, move_base_goal.pose.orientation.y, move_base_goal.pose.orientation.z, move_base_goal.pose.orientation.w ]))[0] ] current_js = rospy.wait_for_message('whole_body_controller/state', JointTrajectoryControllerState) goal = FollowJointTrajectoryGoal() goal.trajectory.header = move_base_goal.header goal.trajectory.joint_names = current_js.joint_names p = JointTrajectoryPoint() p.time_from_start = rospy.Duration(0.1) p.positions = current_js.actual.positions goal.trajectory.points.append(p) last_p = p def next_p(goal_p, last_p, f): diff = (goal_p - last_p) if diff > 0: diff = min(diff, self.max_vel * f) else: diff = max(diff, -self.max_vel * f) return last_p + (diff) while not np.allclose(np.array(goal_js), p.positions[:3]): p = JointTrajectoryPoint() p.time_from_start = last_p.time_from_start + rospy.Duration( 1 * self.freq) p.positions = list(current_js.actual.positions) p.positions[0] = next_p(goal_js[0], last_p.positions[0], self.freq) p.positions[1] = next_p(goal_js[1], last_p.positions[1], self.freq) p.positions[2] = next_p(goal_js[2], last_p.positions[2], self.freq) goal.trajectory.points.append(p) last_p = p self.client.send_goal(goal) while not self.client.wait_for_result(rospy.Duration(.1)): if self.server.is_preempt_requested(): rospy.loginfo('new goal, cancel old one') self.client.cancel_all_goals() self.server.set_preempted(MoveBaseResult()) break # result = self.client.get_result() # if result.error_code == result.SUCCESSFUL: self.server.set_succeeded(MoveBaseResult())
def move_base_cb(self, goal): pwcs = PoseWithCovarianceStamped() pwcs.header = goal.target_pose.header pwcs.pose.pose = goal.target_pose.pose if self.move_base_mode == "beam": rospy.loginfo("move_base: beaming robot to new goal") self.initalpose_callback(pwcs) elif self.move_base_mode == "linear_nav": move_base_linear_action_name = "/move_base_linear" ac_move_base_linear = actionlib.SimpleActionClient(move_base_linear_action_name, MoveBaseAction) rospy.loginfo("Waiting for ActionServer: %s", move_base_linear_action_name) if not ac_move_base_linear.wait_for_server(rospy.Duration(1)): rospy.logerr("Emulator move_base failed because move_base_linear action server is not available") self.as_move_base.set_aborted() return rospy.loginfo("send goal to %s", move_base_linear_action_name) ac_move_base_linear.send_goal(goal) ac_move_base_linear.wait_for_result() ac_move_base_linear_status = ac_move_base_linear.get_state() ac_move_base_linear_result = ac_move_base_linear.get_result() if ac_move_base_linear_status != GoalStatus.SUCCEEDED: rospy.logerr("Emulator move_base failed because move_base_linear failed") self.as_move_base.set_aborted() return else: rospy.logerr("Invalid move_base_action_mode") self.as_move_base.set_aborted() return rospy.loginfo("Emulator move_base succeeded") self.as_move_base.set_succeeded(MoveBaseResult())
def test_happy_flow(self): """ The robot is told to go charge. It should then navigate to a position close to the charger, call the dock-service then uses it's arm to plug itself in and say something when finally getting some juice from the charger. """ self.human_to_robot_speech.publish('Go charge') nav_goal = (10, 10, 0) # Just a random goal in x, y and orientation move_base_goal = self.move_base.await_goal( marker='start_move_to_charger') navigated_to_goal = any( self.move_base.match_in_received_goals( [nav_goal], key=self.move_base.goal_to_xy_theta)) already_at_goal = self.move_base.pose_bl == nav_goal assert navigated_to_goal or already_at_goal, "Robot is not at goal for service" self.move_base.direct_reply(MoveBaseResult(), marker='end_move_to_charger') self.dock.reply(SetStringResponse(success=True), marker='dock_into_charger') self.arm.reply(FollowJointTrajectoryResult( error_code=FollowJointTrajectoryResult.SUCCESSFUL), marker='arm_plug') while not self.say.match_in_received_goals( ["Ah, some juice!", "Jummy, fresh juice!"], key=lambda req: req.text) and not rospy.is_shutdown(): rospy.sleep(0.5) rospy.loginfo("test_happy_flow succeeded")
class W2WMissionPlanner(object): # create messages that are used to publish feedback/result _feedback = MoveBaseFeedback() _result = MoveBaseResult() def __init__(self, name): self._action_name = name #self.heading_offset = rospy.get_param('~heading_offsets', 5.) self.planner_as_name = rospy.get_param('~path_planner_as') self.path_topic = rospy.get_param('~path_topic') self.wp_topic = rospy.get_param('~wp_topic') self.map_frame = rospy.get_param('~map_frame', 'map') # The waypoints as a path rospy.Subscriber(self.path_topic, Path, self.path_cb, queue_size=1) self.latest_path = Path() # LC waypoints, individually rospy.Subscriber(self.wp_topic, PoseStamped, self.wp_cb, queue_size=1) # The client to send each wp to the server self.ac = actionlib.SimpleActionClient(self.planner_as_name, MoveBaseAction) while not self.ac.wait_for_server( rospy.Duration(1)) and not rospy.is_shutdown(): rospy.loginfo("Waiting for action client: %s", self.planner_as_name) rospy.loginfo("Action client connected: %s", self.planner_as_name) while not rospy.is_shutdown(): if self.latest_path.poses: # Get next waypoint in path rospy.loginfo("Sending WP") wp = self.latest_path.poses[0] del self.latest_path.poses[0] # TODO: normalize quaternions here according to rviz warning? goal = MoveBaseGoal(wp) goal.target_pose.header.frame_id = self.map_frame self.ac.send_goal(goal) self.ac.wait_for_result() rospy.loginfo("WP reached, moving on to next one") else: rospy.Rate(1).sleep() def path_cb(self, path_msg): self.latest_path = path_msg rospy.loginfo("Path received with number of wp: %d", len(self.latest_path.poses)) def wp_cb(self, wp_msg): # Waypoints for LC from the backseat driver rospy.loginfo("LC wp received") self.latest_path.poses.insert(0, wp_msg)
def execute(self, goal): rospy.loginfo("Bug algorithm goal: x:{} y:{}".format(goal.target_pose.pose.position.x, goal.target_pose.pose.position.x)) rate = rospy.Rate(self.controller_freq) waypoint = goal.target_pose dist, yaw_error = self.waypoint_info(waypoint) # Calculate timeout start_time = rospy.Time.now().to_sec() timeout = rospy.Duration(dist / 0.3 * 1.2 + 3).to_sec() rospy.loginfo("dist: {}, now: {}, timeout: {}, dt: {}".format(dist, start_time, timeout, rospy.Duration(dist / 0.3 * 1.1 + 3).to_sec())) while dist > self.distance_tolerance: # If preemt if self.server.is_preempt_requested(): rospy.loginfo("PREEMPT bug_algorithm") self.server.set_preempted() return # If timeout if rospy.Time.now().to_sec() - start_time > timeout: rospy.loginfo("ABORTED bug algorithm") self.server.set_aborted() return try: # Calculate waypoint data dist, yaw_error = self.waypoint_info(waypoint) # print("dist: {:.3f} yaw_err:{:.2f}".format(dist, yaw_error)) # Switch states if dist < self.distance_tolerance: break # Waypoint reached # Sum sonar and bump sensor vectors vel_x, ang_z = self.get_range_vector_sum() # Return to heading P controller ang_z -= clamp(0.8 * yaw_error, -self.max_heading_fix, self.max_heading_fix) # Publish cmd_vel # self.cmd_vel_msg.linear.x = (vel_x + self.rc_speed) * self.power_coef # Testing self.cmd_vel_msg.linear.x = (vel_x + 0.35) * self.power_coef # Event self.cmd_vel_msg.angular.z = ang_z * self.power_coef self.cmd_vel_msg.linear.x = clamp(self.cmd_vel_msg.linear.x, -self.max_vel_x, self.max_vel_x) self.cmd_vel_msg.angular.z = clamp(self.cmd_vel_msg.angular.z, -self.max_ang_z, self.max_ang_z) self.cmd_vel_pub.publish(self.cmd_vel_msg) # print("PUB: {:.3f}, {:.3f}".format(self.cmd_vel_msg.linear.x, self.cmd_vel_msg.angular.z)) except Exception as e: rospy.logerr("HOPE NO CRASH: {}".format(e)) rate.sleep() rospy.loginfo("Done bug algorithm") self.server.set_succeeded(result=MoveBaseResult())
def test_unhappy_flow(self): """ The robot is told to go charge. It should then navigate to a position close to the charger. call the dock-service then uses it's arm to plug itself in and say something when finally getting some juice from the charger. However, the docking fails first so the robot retries and then the arm fails at first, so the robot says something about it's arm and then retries """ self.human_to_robot_speech.publish('Go charge') nav_goal = (10, 10, 0) # Just a random goal in x, y and orientation move_base_goal = self.move_base.await_goal() navigated_to_goal = any( self.move_base.match_in_received_goals( [nav_goal], key=self.move_base.goal_to_xy_theta)) already_at_goal = self.move_base.pose_bl == nav_goal assert navigated_to_goal or already_at_goal, "Robot is not at goal for service" self.move_base.direct_reply(MoveBaseResult()) # Using a custom reply, we can override the default reply set on the dock service temporarily with self.dock.custom_reply(): self.dock.reply(SetStringResponse(success=False)) # the robot notices docking has failed, so it should say something about that say_goal = self.say.await_goal() assert 'fail' in say_goal.text self.say.direct_reply(SayResult(success=True)) # Then retry docking, now successfully self.dock.reply(SetStringResponse(success=True)) with self.arm.custom_reply(): self.arm.reply( FollowJointTrajectoryResult( error_code=FollowJointTrajectoryResult. PATH_TOLERANCE_VIOLATED)) # the robot notices docking has failed, so it should say something about that say_goal = self.say.await_goal() assert 'fail' in say_goal.text self.say.direct_reply(SayResult(success=True)) self.arm.reply( FollowJointTrajectoryResult( error_code=FollowJointTrajectoryResult.SUCCESSFUL)) while not self.say.match_in_received_goals( ["Ah, some juice!", "Jummy, fresh juice!"], key=lambda req: req.text) and not rospy.is_shutdown(): rospy.sleep(0.5) rospy.loginfo("test_unhappy_flow succeeded")
def cb(self, move_base_goal): """ Callback for nav_p action server :param move_base_goal: goal position for the nav_p controller :type move_base_goal: MoveBaseGoal """ move_base_goal = self.tf.transform_pose(self.odom_frame, move_base_goal.target_pose) self.tf.set_frame_from_pose(self.robot_root, move_base_goal) self.server.set_succeeded(MoveBaseResult())
def execute_cb(self, goal): rospy.loginfo("Emergency action initiated") r = rospy.Rate(11.) # 10hz while not rospy.is_shutdown(): # Preempted if self._as.is_preempt_requested(): # Publish emergency command self.emergency_pub.publish(False) #Disable controllers self.lcg_pid_enable.publish(True) self.vbs_pid_enable.publish(True) self.tcg_pid_enable.publish(True) self.yaw_pid_enable.publish(True) self.depth_pid_enable.publish(True) self.vel_pid_enable.publish(True) rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted(MoveBaseResult(), "Preempted EmergencySurface action") return # Publish emergency command self.emergency_pub.publish(True) #Disable controllers self.lcg_pid_enable.publish(False) self.vbs_pid_enable.publish(False) self.tcg_pid_enable.publish(False) self.yaw_pid_enable.publish(False) self.depth_pid_enable.publish(False) self.vel_pid_enable.publish(False) #set VBS to 0 vbs_level = PercentStamped() vbs_level.value = 0.0 self.vbs_pub.publish(vbs_level) # Stop thrusters rpm = ThrusterRPMs() rpm.thruster_1_rpm = 0.0 rpm.thruster_2_rpm = 0.0 self.rpm_pub.publish(rpm) r.sleep() rospy.loginfo('%s: Completed' % self._action_name)
def move_base_cb(self, goal): rospy.loginfo('move_base_cb') self.target_position = goal.target_pose.pose.position self.target_orientation = goal.target_pose.pose.orientation self.moves_base = True while not self.reply: rospy.sleep(0.2) (trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0)) self.robot_pose_.pose.position.x = trans[0] self.robot_pose_.pose.position.y = trans[1] feedback = MoveBaseFeedback() feedback.base_position.pose.position.x = \ self.robot_pose_.pose.position.x feedback.base_position.pose.position.y = \ self.robot_pose_.pose.position.y self.move_base_as_.publish_feedback(feedback) if self.move_base_as_.is_preempt_requested(): self.preempted += 1 rospy.loginfo("Preempted!") self.moves_base = False self.move_base_as_.set_preempted() return None if self.move_base_as_.is_new_goal_available(): self.preempted += 1 self.move_base_cb(self.move_base_as_.accept_new_goal()) return None else: result = MoveBaseResult() self.reply = False self.preempted = 0 self.moves_base = False self.target_position = Point() self.target_orientation = Quaternion() if self.navigation_succedes: self.move_base_as_.set_succeeded(result) else: self.move_base_as_.set_aborted(result)
def move_base_cb(self, req): self.base_client.set_succeeded(MoveBaseResult(), "Goal reached.")
class BackseatDriver(object): # create messages that are used to publish feedback/result _feedback = MoveBaseFeedback() _result = MoveBaseResult() def __init__(self, name): self._action_name = name #self.heading_offset = rospy.get_param('~heading_offsets', 5.) # self.planner_as_name = rospy.get_param('~path_planner_as') self.path_topic = rospy.get_param('~path_topic') self.map_frame = rospy.get_param('~map_frame', 'map') self.base_frame = rospy.get_param('~base_frame', 'base_link') self.avg_pose_top = rospy.get_param("~average_pose_topic", '/average_pose') self.cov_threshold = rospy.get_param("~cov_threshold", 50) self.wp_topic = rospy.get_param('~wp_topic') self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.) # To send LC wp to the mission planner self.wp_pub = rospy.Publisher(self.wp_topic, PoseStamped, queue_size=1) self.listener = tf.TransformListener() # The waypoints as a path rospy.Subscriber(self.path_topic, Path, self.path_cb, queue_size=1) self.latest_path = Path() # The PF filter state rospy.Subscriber(self.avg_pose_top, PoseWithCovarianceStamped, self.pf_cb, queue_size=1) self.closing_loop = False self.new_wp = PoseStamped() # The LC waypoints, as a path self.lc_waypoints = Path() self.lc_waypoints.header.frame_id = self.map_frame # One LC wp for testing lc_wp = PoseStamped() lc_wp.header.frame_id = self.map_frame lc_wp.pose.position.x = -183.65 lc_wp.pose.position.y = -332.39 lc_wp.pose.position.z = 0. lc_wp.pose.orientation.w = 1 self.lc_waypoints.poses.append(lc_wp) rospy.spin() def path_cb(self, path_msg): self.latest_path = path_msg rospy.loginfo("Path received with number of wp: %d", len(self.latest_path.poses)) def pf_cb(self, pf_msg): # Reconstruct covariance self.cov = np.zeros((6, 6)) for i in range(3): for j in range(3): self.cov[i, j] = pf_msg.pose.covariance[i * 3 + j] # Reconstruct pose estimate position_estimate = pf_msg.pose.pose.position # Monitor trace trc = np.sum(np.diag(self.cov)) if trc > self.cov_threshold and not self.closing_loop: # Pose uncertainty too high, closing the loop to relocalize # Find LC wp between current PF pose and next wp on the survey self.new_wp = self.lc_waypoints.poses[0] # Preempt current waypoint/path self.wp_pub.publish(self.new_wp) rospy.loginfo("Sent LC waypoint") self.closing_loop = True elif self.closing_loop: rospy.loginfo("Going for a loop closure!") try: (trans, rot) = self.listener.lookupTransform(self.map_frame, self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return start_pos = np.array(trans) end_pos = np.array([ self.new_wp.pose.position.x, self.new_wp.pose.position.y, self.new_wp.pose.position.z ]) rospy.loginfo("BS driver diff " + str(np.linalg.norm(start_pos - end_pos))) if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance: # Goal reached rospy.loginfo("Loop closed!") self.closing_loop = False
def execute_callback(self, move_base_goal): self.goal = move_base_goal.target_pose.pose # Set the provided goal as the current goal rospy.logdebug('[Move Base Fake] Execute Callback: {}'.format( str(self.goal.position))) self.valid_goal = True # Assume it is valid self.current_goal_started = False # It hasnt started yet self.current_goal_complete = False # It hasnt been completed r = rospy.Rate(1) while not rospy.is_shutdown(): # Always start by checking if there is a new goal that preempts the current one if self.action_server.is_preempt_requested(): # Tell Bug algorithm to stop before changing or stopping goal self.pub_state_to_bug(False, self.bugType) # Stop bug algorithm if self.action_server.is_new_goal_available(): new_goal = self.action_server.accept_new_goal( ) # There is a new goal rospy.logdebug('[Move Base Fake] New Goal: {}'.format( str(self.goal.position))) self.goal = new_goal.target_pose.pose # Set the provided goal as the current goal self.valid_goal = True # Assume it is valid self.current_goal_started = False # It hasnt started yet self.current_goal_complete = False # It hasnt been completed else: self.action_server.set_preempted( ) # No new goal, we've just been told to stop self.goal = None # Stop everything self.valid_goal = False self.current_goal_started = False self.current_goal_complete = False return # Start goal if self.valid_goal and not self.current_goal_started: rospy.logdebug('[Move Base Fake] Starting Goal') #set the goal self.pub_goal_to_bug(self.goal, self.bugType) #start to go to the goal self.pub_state_to_bug(True, self.bugType) self.current_goal_started = True # Only start once # Feedback ever loop just reports current location feedback = MoveBaseFeedback() feedback.base_position.pose.position = self.position self.action_server.publish_feedback(feedback) # Completed is set in a callback that you need to link to a service or subscriber if self.current_goal_complete: rospy.logdebug('[Move Base Fake] Finishing Goal') #stop the bug algorithm self.pub_state_to_bug(False, self.bugType) self.goal = None # Stop everything self.valid_goal = False self.current_goal_started = False self.current_goal_complete = False self.action_server.set_succeeded( MoveBaseResult(), 'Goal reached') # Send success message return r.sleep() # Shutdown rospy.logdebug('[Move Base Fake] Shutting Down') self.pub_state_to_bug(False, self.bugType) self.goal = None # Stop everything self.valid_goal = False self.current_goal_started = False self.current_goal_complete = False
def execute_cb(goal): rospy.loginfo("Received a goal") # check for network cable if network_connected: server.set_aborted( None, "Dangerous to navigate with network cable connected.") return # check for power cable if ac_power_connected: server.set_aborted( None, "Dangerous to navigate with AC power cable connected.") return # check for power cable # TODO # start the arms tucking tuck_arms_client = actionlib.SimpleActionClient('tuck_arms', TuckArmsAction) tuck_arms_client.wait_for_server() tuck_goal = TuckArmsGoal() tuck_goal.tuck_left = True tuck_goal.tuck_right = True tuck_arms_client.send_goal(tuck_goal) # start the torso lowering torso_client = actionlib.SimpleActionClient( 'torso_controller/position_joint_action', SingleJointPositionAction) torso_client.wait_for_server() torso_down_goal = SingleJointPositionGoal() torso_down_goal.position = 0 torso_client.send_goal(torso_down_goal) # configure the tilting laser #if not set_tilt_profile([1.05, -.7, 1.05], [0.0, 1.8, 2.0125 + .3]): if not set_tilt_profile([0.1, 0.1, 0.1], [0.0, 2, 4]): server.set_aborted(MoveBaseResult(), "Couldn't set the profile on the laser") return configure_laser() configure_head() # wait for tuck_arms to finish. (Don't wait for torso to lower, that is a Nice To Have) while not tuck_arms_client.wait_for_result(rospy.Duration(0.1)): if server.is_preempt_requested(): if not server.is_new_goal_available(): tuck_arms_client.cancel_goal() tuck_state = tuck_arms_client.get_state() if tuck_state != GoalStatus.SUCCEEDED: if tuck_state == GoalStatus.PREEMPTED: server.set_preempted(MoveBaseResult(), "Tuck-arms preempted") elif tuck_state == GoalStatus.ABORTED: server.set_aborted(MoveBaseResult(), "Tuck-arms aborted") return # Now everything should be ready so send the navigation goal. move_base_client.send_goal(goal, None, None, feedback_cb) while not move_base_client.wait_for_result(rospy.Duration(0.1)): #in the unlikely event of network cable being plugged in while moving, stop moving. if network_connected: move_base_client.cancel_goal() server.set_aborted( None, "Dangerous to navigate with network cable connected.") return #in the unlikely event of ac power cable being plugged in while moving, stop moving. if ac_power_connected: move_base_client.cancel_goal() server.set_aborted( None, "Dangerous to navigate with ac power cable connected.") return if server.is_preempt_requested(): if not server.is_new_goal_available(): rospy.loginfo( "Preempt requested without new goal, cancelling move_base goal." ) move_base_client.cancel_goal() server.set_preempted(MoveBaseResult(), "Got preempted by a new goal") return terminal_state = move_base_client.get_state() result = move_base_client.get_result() if terminal_state == GoalStatus.PREEMPTED: server.set_preempted(result) elif terminal_state == GoalStatus.SUCCEEDED: server.set_succeeded(result) elif terminal_state == GoalStatus.ABORTED: server.set_aborted(result) else: server.set_aborted(result, "Unknown result from move_base")
def do_move_base(goal): rospy.sleep(2) result = MoveBaseResult() server.set_succeeded(result)
class P2PPlanner(object): # create messages that are used to publish feedback/result _feedback = MoveBaseFeedback() _result = MoveBaseResult() def execute_cb(self, goal): # helper variables #r = rospy.Rate(1) rospy.loginfo("Goal received") success = True self.nav_goal = goal.target_pose.pose r = rospy.Rate(10.) # 10hz counter = 0 while not rospy.is_shutdown() and self.nav_goal is not None: # Preempted if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() success = False self.nav_goal = None # Stop thrusters rpm = ThrusterRPMs() rpm.thruster_1_rpm = 0. self.rpm_pub.publish(rpm) break # Publish feedback if counter % 100 == 0: try: (trans, rot) = self.listener.lookupTransform( "/world", "sam/base_link", rospy.Time(0)) pose_fb = PoseStamped() pose_fb.header.frame_id = "/world" pose_fb.pose.position.x = trans[0] pose_fb.pose.position.y = trans[1] pose_fb.pose.position.z = trans[2] self._feedback.base_position = pose_fb self._feedback.base_position.header.stamp = rospy.get_rostime( ) self._as.publish_feedback(self._feedback) rospy.loginfo("Sending feedback") except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("Error with tf") continue # Thruster forward rpm = ThrusterRPMs() rpm.thruster_1_rpm = 1000. self.rpm_pub.publish(rpm) rospy.loginfo("Thrusters forward") counter += 1 r.sleep() if success: # Stop thruster rpm = ThrusterRPMs() rpm.thruster_1_rpm = -1000.0 cnt = 0 while not rospy.is_shutdown() and cnt < 50: self.rpm_pub.publish(rpm) cnt += 1 r.sleep() #self._result.sequence = self._feedback.sequence rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result) def timer_callback(self, event): if self.nav_goal is None: rospy.loginfo("Nav goal is None!") return try: (trans, rot) = self.listener.lookupTransform("/world", self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return # TODO: we could use this code for the other check also goal_point = PointStamped() goal_point.header.frame_id = "/world" goal_point.header.stamp = rospy.Time(0) goal_point.point.x = self.nav_goal.position.x goal_point.point.y = self.nav_goal.position.y goal_point.point.z = self.nav_goal.position.z try: goal_point_base = self.listener.transformPoint( self.base_frame, goal_point) if goal_point_base.point.x < 0.: rospy.loginfo("Ahead of goal, returning success!") self.nav_goal = None return except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): pass #print("Checking if nav goal is reached!") current_pos = np.array(trans) current_pos[2] = 0.0 end_pos = np.array( [self.nav_goal.position.x, self.nav_goal.position.y, 0.]) if np.linalg.norm(current_pos - end_pos) < self.goal_tolerance: rospy.loginfo("Reached goal!") self.nav_goal = None #else: # print("Did not reach nav goal!") def __init__(self, name): """Plot an example bezier curve.""" self._action_name = name self.heading_offset = rospy.get_param('~heading_offsets', 5.) self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.) self.base_frame = rospy.get_param('~base_frame', "lolo_auv_1/base_link") self.nav_goal = None self.listener = tf.TransformListener() rospy.Timer(rospy.Duration(2), self.timer_callback) self.rpm_pub = rospy.Publisher('/uavcan_rpm_command', ThrusterRPMs, queue_size=10) self._as = actionlib.SimpleActionServer(self._action_name, MoveBaseAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() rospy.loginfo("Announced action server with name: %s", self._action_name) r = rospy.Rate(10) # 10hz rospy.spin()
class WPDepthPlanner(object): # create messages that are used to publish feedback/result _feedback = MoveBaseFeedback() _result = MoveBaseResult() def create_marker(self, yaw_setpoint, depth_setpoint): self.marker.header.frame_id = "/sam/odom" self.marker.header.stamp = rospy.Time(0) self.marker.ns = "/sam/viz" self.marker.id = 0 self.marker.type = 0 self.marker.action = 0 self.marker.pose.position.x = self._feedback.base_position.pose.position.x self.marker.pose.position.y = self._feedback.base_position.pose.position.y self.marker.pose.position.z = self._feedback.base_position.pose.position.z q = quaternion_from_euler(0,0,yaw_setpoint) self.marker.pose.orientation.x = q[0] self.marker.pose.orientation.y = q[1] self.marker.pose.orientation.z = q[2] self.marker.pose.orientation.w = q[3] self.marker.scale.x = 1 self.marker.scale.y = 0.1 self.marker.scale.z = 0.1 self.marker.color.a = 1.0 # Dont forget to set the alpha! self.marker.color.r = 1.0 self.marker.color.g = 1.0 self.marker.color.b = 1.0 self.marker_pub.publish(self.marker) def yaw_feedback_cb(self,yaw_feedback): self.yaw_feedback= yaw_feedback.data def angle_wrap(self,angle): if(abs(angle)>3.141516): angle= angle - (abs(angle)/angle)*2*3.141516; #Angle wrapping between -pi and pi rospy.loginfo_throttle_identical(20, "Angle Error Wrapped") return angle def turbo_turn(self,angle_error): rpm = self.turbo_turn_rpm rudder_angle = self.rudder_angle flip_rate = self.flip_rate left_turn = True #left turn increases value of yaw angle towards pi, right turn decreases it towards -pi. if angle_error < 0: left_turn = False rospy.loginfo('Right turn!') rospy.loginfo('Turbo Turning!') if left_turn: rudder_angle = -rudder_angle thrust_rate = 11. rate = rospy.Rate(thrust_rate) self.vec_pub.publish(0., rudder_angle, Header()) loop_time = 0. dualrpm = DualThrusterRPM() while not rospy.is_shutdown() and loop_time < .37/flip_rate: dualrpm.thruster_front.rpm = rpm dualrpm.thruster_back.rpm = rpm self.rpm_pub.publish(dualrpm) loop_time += 1./thrust_rate rate.sleep() self.vec_pub.publish(0., -rudder_angle, Header()) loop_time = 0. while not rospy.is_shutdown() and loop_time < .63/flip_rate: dualrpm.thruster_front.rpm = -rpm dualrpm.thruster_back.rpm = -rpm self.rpm_pub.publish(dualrpm) loop_time += 1./thrust_rate rate.sleep() def execute_cb(self, goal): rospy.loginfo("Goal received") success = True self.nav_goal = goal.target_pose.pose self.nav_goal_frame = goal.target_pose.header.frame_id if self.nav_goal_frame is None or self.nav_goal_frame == '': rospy.logwarn("Goal has no frame id! Using utm by default") self.nav_goal_frame = 'utm' goal_point = PointStamped() goal_point.header.frame_id = self.nav_goal_frame goal_point.header.stamp = rospy.Time(0) goal_point.point.x = self.nav_goal.position.x goal_point.point.y = self.nav_goal.position.y goal_point.point.z = self.nav_goal.position.z try: goal_point_local = self.listener.transformPoint(self.nav_goal_frame, goal_point) self.nav_goal.position.x = goal_point_local.point.x self.nav_goal.position.y = goal_point_local.point.y self.nav_goal.position.z = goal_point_local.point.z except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print ("Not transforming point to world local") pass rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x) r = rospy.Rate(11.) # 10hz counter = 0 while not rospy.is_shutdown() and self.nav_goal is not None: self.yaw_pid_enable.publish(True) self.depth_pid_enable.publish(True) # Preempted if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) success = False self.nav_goal = None # Stop thrusters rpm = DualThrusterRPM() rpm.thruster_front.rpm = 0. rpm.thruster_back.rpm = 0. self.rpm_pub.publish(rpm) self.yaw_pid_enable.publish(False) self.depth_pid_enable.publish(False) self.vbs_pid_enable.publish(False) self.vel_pid_enable.publish(False) print('wp depth action planner: stopped thrusters') self._as.set_preempted(self._result, "Preempted WP action") return # Publish feedback if counter % 5 == 0: try: (trans, rot) = self.listener.lookupTransform(self.nav_goal_frame, self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("Error with tf:"+str(self.nav_goal_frame) + " to "+str(self.base_frame)) continue pose_fb = PoseStamped() pose_fb.header.frame_id = self.nav_goal_frame pose_fb.pose.position.x = trans[0] pose_fb.pose.position.y = trans[1] pose_fb.pose.position.z = trans[2] self._feedback.base_position = pose_fb self._feedback.base_position.header.stamp = rospy.get_rostime() self._as.publish_feedback(self._feedback) #rospy.loginfo("Sending feedback") #Compute yaw setpoint. xdiff = self.nav_goal.position.x - pose_fb.pose.position.x ydiff = self.nav_goal.position.y - pose_fb.pose.position.y yaw_setpoint = math.atan2(ydiff,xdiff) print('xdiff:',xdiff,'ydiff:',ydiff,'yaw_setpoint:',yaw_setpoint) #compute yaw_error (e.g. for turbo_turn) yaw_error= -(self.yaw_feedback - yaw_setpoint) yaw_error= self.angle_wrap(yaw_error) #wrap angle error between -pi and pi depth_setpoint = self.nav_goal.position.z self.depth_pub.publish(depth_setpoint) #self.vbs_pid_enable.publish(False) #self.vbs_pub.publish(depth_setpoint) if self.vel_ctrl_flag: rospy.loginfo_throttle_identical(5, "vel ctrl, no turbo turn") #with Velocity control self.yaw_pid_enable.publish(True) self.yaw_pub.publish(yaw_setpoint) # Publish to velocity controller self.vel_pid_enable.publish(True) self.vel_pub.publish(self.vel_setpoint) self.roll_pub.publish(self.roll_setpoint) #rospy.loginfo("Velocity published") else: if self.turbo_turn_flag: #if turbo turn is included rospy.loginfo("Yaw error: %f", yaw_error) if abs(yaw_error) > self.turbo_angle_min and abs(yaw_error) < self.turbo_angle_max: #turbo turn with large deviations, maximum deviation is 3.0 radians to prevent problems with discontinuities at +/-pi self.yaw_pid_enable.publish(False) self.turbo_turn(yaw_error) self.depth_pid_enable.publish(False) self.vbs_pid_enable.publish(True) self.vbs_pub.publish(depth_setpoint) else: rospy.loginfo_throttle_identical(5,"Normal WP following") #normal turning if the deviation is small self.vbs_pid_enable.publish(False) self.depth_pid_enable.publish(True) self.yaw_pid_enable.publish(True) self.yaw_pub.publish(yaw_setpoint) self.create_marker(yaw_setpoint,depth_setpoint) # Thruster forward rpm = DualThrusterRPM() rpm.thruster_front.rpm = self.forward_rpm rpm.thruster_back.rpm = self.forward_rpm self.rpm_pub.publish(rpm) #rospy.loginfo("Thrusters forward") else: #turbo turn not included, no velocity control rospy.loginfo_throttle_identical(5, "Normal WP following, no turbo turn") self.yaw_pid_enable.publish(True) self.yaw_pub.publish(yaw_setpoint) self.create_marker(yaw_setpoint,depth_setpoint) # Thruster forward rpm = DualThrusterRPM() rpm.thruster_front.rpm = self.forward_rpm rpm.thruster_back.rpm = self.forward_rpm self.rpm_pub.publish(rpm) #rospy.loginfo("Thrusters forward") counter += 1 r.sleep() # Stop thruster self.vel_pid_enable.publish(False) rpm = DualThrusterRPM() rpm.thruster_front.rpm = 0.0 rpm.thruster_back.rpm = 0.0 self.rpm_pub.publish(rpm) #Stop controllers self.yaw_pid_enable.publish(False) self.depth_pid_enable.publish(False) self.vbs_pid_enable.publish(False) self.vel_pid_enable.publish(False) rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result) def timer_callback(self, event): if self.nav_goal is None: #rospy.loginfo_throttle(30, "Nav goal is None!") return try: (trans, rot) = self.listener.lookupTransform(self.nav_goal_frame, self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return # TODO: we could use this code for the other check also goal_point = PointStamped() goal_point.header.frame_id = self.nav_goal_frame goal_point.header.stamp = rospy.Time(0) goal_point.point.x = self.nav_goal.position.x goal_point.point.y = self.nav_goal.position.y goal_point.point.z = self.nav_goal.position.z #print("Checking if nav goal is reached!") start_pos = np.array(trans) end_pos = np.array([self.nav_goal.position.x, self.nav_goal.position.y, self.nav_goal.position.z]) # We check for success out of the main control loop in case the main control loop is # running at 300Hz or sth. like that. We dont need to check succes that frequently. xydiff = start_pos[:2] - end_pos[:2] zdiff = np.abs(np.abs(start_pos[2]) - np.abs(end_pos[2])) xydiff_norm = np.linalg.norm(xydiff) # rospy.logdebug("diff xy:"+ str(xydiff_norm)+' z:' + str(zdiff)) rospy.loginfo("diff xy:"+ str(xydiff_norm)+' z:' + str(zdiff)+ " WP tol:"+ str(self.wp_tolerance)+ "Depth tol:"+str(self.depth_tolerance)) if xydiff_norm < self.wp_tolerance and zdiff < self.depth_tolerance: rospy.loginfo("Reached goal!") self.nav_goal = None def __init__(self, name): """Publish yaw and depth setpoints based on waypoints""" self._action_name = name #self.heading_offset = rospy.get_param('~heading_offsets', 5.) self.wp_tolerance = rospy.get_param('~wp_tolerance', 5.) self.depth_tolerance = rospy.get_param('~depth_tolerance', 0.5) self.base_frame = rospy.get_param('~base_frame', "sam/base_link") rpm_cmd_topic = rospy.get_param('~rpm_cmd_topic', '/sam/core/thrusters_cmd') heading_setpoint_topic = rospy.get_param('~heading_setpoint_topic', '/sam/ctrl/dynamic_heading/setpoint') yaw_pid_enable_topic = rospy.get_param('~yaw_pid_enable_topic', '/sam/ctrl/dynamic_heading/pid_enable') depth_setpoint_topic = rospy.get_param('~depth_setpoint_topic', '/sam/ctrl/dynamic_depth/setpoint') depth_pid_enable_topic = rospy.get_param('~depth_pid_enable_topic', '/sam/ctrl/dynamic_depth/pid_enable') self.forward_rpm = int(rospy.get_param('~forward_rpm', 1000)) #related to turbo turn self.turbo_turn_flag = rospy.get_param('~turbo_turn_flag', False) thrust_vector_cmd_topic = rospy.get_param('~thrust_vector_cmd_topic', '/sam/core/thrust_vector_cmd') yaw_feedback_topic = rospy.get_param('~yaw_feedback_topic', '/sam/ctrl/yaw_feedback') self.turbo_angle_min_deg = rospy.get_param('~turbo_angle_min', 90) self.turbo_angle_min = np.radians(self.turbo_angle_min_deg) self.turbo_angle_max = 3.0 self.flip_rate = rospy.get_param('~flip_rate', 0.5) self.rudder_angle = rospy.get_param('~rudder_angle', 0.08) self.turbo_turn_rpm = rospy.get_param('~turbo_turn_rpm', 1000) vbs_pid_enable_topic = rospy.get_param('~vbs_pid_enable_topic', '/sam/ctrl/vbs/pid_enable') vbs_setpoint_topic = rospy.get_param('~vbs_setpoint_topic', '/sam/ctrl/vbs/setpoint') #related to velocity regulation instead of rpm self.vel_ctrl_flag = rospy.get_param('~vel_ctrl_flag', False) self.vel_setpoint = rospy.get_param('~vel_setpoint', 0.5) #velocity setpoint in m/s self.roll_setpoint = rospy.get_param('~roll_setpoint', 0) vel_setpoint_topic = rospy.get_param('~vel_setpoint_topic', '/sam/ctrl/dynamic_velocity/u_setpoint') roll_setpoint_topic = rospy.get_param('~roll_setpoint_topic', '/sam/ctrl/dynamic_velocity/roll_setpoint') vel_pid_enable_topic = rospy.get_param('~vel_pid_enable_topic', '/sam/ctrl/dynamic_velocity/pid_enable') self.nav_goal = None self.listener = tf.TransformListener() rospy.Timer(rospy.Duration(0.5), self.timer_callback) self.yaw_feedback=0 rospy.Subscriber(yaw_feedback_topic, Float64, self.yaw_feedback_cb) self.rpm_pub = rospy.Publisher(rpm_cmd_topic, DualThrusterRPM, queue_size=10) self.yaw_pub = rospy.Publisher(heading_setpoint_topic, Float64, queue_size=10) self.depth_pub = rospy.Publisher(depth_setpoint_topic, Float64, queue_size=10) self.vel_pub = rospy.Publisher(vel_setpoint_topic, Float64, queue_size=10) self.roll_pub = rospy.Publisher(roll_setpoint_topic, Float64, queue_size=10) #TODO make proper if it works. self.vbs_pub = rospy.Publisher(vbs_setpoint_topic, Float64, queue_size=10) self.yaw_pid_enable = rospy.Publisher(yaw_pid_enable_topic, Bool, queue_size=10) self.depth_pid_enable = rospy.Publisher(depth_pid_enable_topic, Bool, queue_size=10) self.vbs_pid_enable = rospy.Publisher(vbs_pid_enable_topic, Bool, queue_size=10) self.vel_pid_enable = rospy.Publisher(vel_pid_enable_topic, Bool, queue_size=10) self.vec_pub = rospy.Publisher(thrust_vector_cmd_topic, ThrusterAngles, queue_size=10) self.marker = Marker() self.marker_pub = rospy.Publisher('/sam/viz/wp_marker', Marker, queue_size=1) self._as = actionlib.SimpleActionServer(self._action_name, MoveBaseAction, execute_cb=self.execute_cb, auto_start = False) self._as.start() rospy.loginfo("Announced action server with name: %s", self._action_name) rospy.spin()
class LeaderFollower(object): # create messages that are used to publish feedback/result _feedback = MoveBaseFeedback() _result = MoveBaseResult() def execute_cb(self, goal): rospy.loginfo_throttle(5, "Goal received") success = True rate = rospy.Rate(11.) # 10hz counter = 0 while not rospy.is_shutdown(): self.yaw_pid_enable.publish(True) self.depth_pid_enable.publish(True) # Preempted if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) success = False # Stop thrusters self.rpm.thruster_1_rpm = 0. self.rpm.thruster_2_rpm = 0. self.rpm_pub.publish(self.rpm) self.yaw_pid_enable.publish(False) self.depth_pid_enable.publish(False) self.vel_pid_enable.publish(False) print('leader_follower_action: stopped thrusters') self._as.set_preempted(self._result, "Preempted WP action") return # Compute and Publish setpoints if counter % 1 == 0: # distance check is done in the BT, we will add CBFs here later, which will include # that distance as a constraint anyways # if sqrt(rel_trans[0]**2 + rel_trans[1]**2 + rel_trans[2]**2) < self.min_dist: # break #Check transform between SAM1 (leader- goal) and SAM2 (follower -self), define a goal point and call the go_to_point() function self.leader_frame = goal.target_pose.header.frame_id try: (follower_trans, follower_rot) = self.listener.lookupTransform( self.follower_odom, self.follower_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException): rospy.logwarn_throttle_identical( 5, "Could not get transform between " + self.leader_frame + " and " + self.follower_frame) success = False break except: rospy.logwarn_throttle_identical( 5, "Could not do tf lookup for some other reason") success = False break try: (leader_trans, leader_rot) = self.listener.lookupTransform( self.follower_odom, self.leader_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException): rospy.logwarn_throttle_identical( 5, "Could not get transform between " + self.leader_frame + " and " + self.follower_frame) success = False break except: rospy.logwarn_throttle_identical( 5, "Could not do tf lookup for some other reason") success = False break xdiff = leader_trans[0] - follower_trans[0] ydiff = leader_trans[1] - follower_trans[1] zdiff = leader_trans[2] - follower_trans[2] yaw_setpoint = math.atan2(ydiff, xdiff) depth_setpoint = -zdiff rospy.loginfo_throttle( 10, 'yaw_setpoint:' + str(yaw_setpoint) + ' depth_setpoint:' + str(depth_setpoint)) self.depth_pub.publish(depth_setpoint) if self.vel_ctrl_flag: rospy.loginfo_throttle_identical( 5, "vel ctrl, no turbo turn") #with Velocity control self.yaw_pid_enable.publish(True) self.yaw_pub.publish(yaw_setpoint) # Publish to velocity controller self.vel_pid_enable.publish(True) self.vel_pub.publish(self.vel_setpoint) self.roll_pub.publish(self.roll_setpoint) #rospy.loginfo("Velocity published") else: #turbo turn not included, no velocity control rospy.loginfo_throttle_identical( 5, "Normal WP following, no turbo turn") self.yaw_pid_enable.publish(True) self.yaw_pub.publish(yaw_setpoint) # Thruster forward self.rpm.thruster_1_rpm = self.forward_rpm self.rpm.thruster_2_rpm = self.forward_rpm self.rpm_pub.publish(self.rpm) #rospy.loginfo("Thrusters forward") counter += 1 rate.sleep() # Stop thruster self.vel_pid_enable.publish(False) self.rpm.thruster_1_rpm = 0.0 self.rpm.thruster_2_rpm = 0.0 self.rpm_pub.publish(self.rpm) #Stop controllers self.yaw_pid_enable.publish(False) self.depth_pid_enable.publish(False) self.vel_pid_enable.publish(False) if self._result: rospy.loginfo('%s: Succeeded' % self._action_name) else: rospy.logwarn_throttle_identical(3, '%s: Failed' % self._action_name) self._as.set_succeeded(self._result) def __init__(self, name): self.rpm = ThrusterRPMs() """Publish yaw and depth setpoints based on waypoints""" self._action_name = name self.follower_frame = rospy.get_param('~follower_frame', '/sam_2/base_link') self.follower_odom = rospy.get_param('~follower_odom', '/sam_2/odom') # self.min_dist = rospy.get_param('~min_dist', 5.) rpm_cmd_topic = rospy.get_param('~rpm_cmd_topic', '/sam/core/rpm_cmd') heading_setpoint_topic = rospy.get_param( '~heading_setpoint_topic', '/sam/ctrl/dynamic_heading/setpoint') yaw_pid_enable_topic = rospy.get_param( '~yaw_pid_enable_topic', '/sam/ctrl/dynamic_heading/pid_enable') depth_setpoint_topic = rospy.get_param( '~depth_setpoint_topic', '/sam/ctrl/dynamic_depth/setpoint') depth_pid_enable_topic = rospy.get_param( '~depth_pid_enable_topic', '/sam/ctrl/dynamic_depth/pid_enable') self.forward_rpm = int(rospy.get_param('~forward_rpm', 1000)) #related to velocity regulation instead of rpm self.vel_ctrl_flag = rospy.get_param('~vel_ctrl_flag', False) self.vel_setpoint = rospy.get_param('~vel_setpoint', 0.5) #velocity setpoint in m/s self.roll_setpoint = rospy.get_param('~roll_setpoint', 0) vel_setpoint_topic = rospy.get_param( '~vel_setpoint_topic', '/sam/ctrl/dynamic_velocity/u_setpoint') roll_setpoint_topic = rospy.get_param( '~roll_setpoint_topic', '/sam/ctrl/dynamic_velocity/roll_setpoint') vel_pid_enable_topic = rospy.get_param( '~vel_pid_enable_topic', '/sam/ctrl/dynamic_velocity/pid_enable') self.listener = tf.TransformListener() self.rpm_pub = rospy.Publisher(rpm_cmd_topic, ThrusterRPMs, queue_size=10) self.yaw_pub = rospy.Publisher(heading_setpoint_topic, Float64, queue_size=10) self.depth_pub = rospy.Publisher(depth_setpoint_topic, Float64, queue_size=10) self.vel_pub = rospy.Publisher(vel_setpoint_topic, Float64, queue_size=10) self.roll_pub = rospy.Publisher(roll_setpoint_topic, Float64, queue_size=10) self.yaw_pid_enable = rospy.Publisher(yaw_pid_enable_topic, Bool, queue_size=10) self.depth_pid_enable = rospy.Publisher(depth_pid_enable_topic, Bool, queue_size=10) self.vel_pid_enable = rospy.Publisher(vel_pid_enable_topic, Bool, queue_size=10) self._as = actionlib.SimpleActionServer(self._action_name, MoveBaseAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() rospy.loginfo("Announced action server with name: %s", self._action_name) rospy.spin()
class W2WPathPlanner(object): # create messages that are used to publish feedback/result _feedback = MoveBaseFeedback() _result = MoveBaseResult() def execute_cb(self, goal): rospy.loginfo("Goal received") success = True self.nav_goal = goal.target_pose.pose self.nav_goal_frame = goal.target_pose.header.frame_id if self.nav_goal_frame is None or self.nav_goal_frame == '': rospy.logwarn("Goal has no frame id! Using map by default") self.nav_goal_frame = self.map_frame r = rospy.Rate(10.) # 10hz counter = 0 while not rospy.is_shutdown() and self.nav_goal is not None: # Preempted if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() self.nav_goal = None # Stop thruster self.motion_command(0., 0., 0.) break # Transform goal map --> base frame goal_point = PointStamped() goal_point.header.frame_id = self.nav_goal_frame goal_point.header.stamp = rospy.Time(0) goal_point.point.x = self.nav_goal.position.x goal_point.point.y = self.nav_goal.position.y goal_point.point.z = self.nav_goal.position.z try: goal_point_local = self.listener.transformPoint( self.base_frame, goal_point) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print("Not transforming point to base frame") pass #Compute throttle error # throttle_level = min(self.max_throttle, np.linalg.norm( # np.array([goal_point_local.point.x + goal_point_local.point.y]))) # Nacho: no real need to adjust the throttle throttle_level = self.max_throttle # Compute thrust error alpha = math.atan2(goal_point_local.point.y, goal_point_local.point.x) sign = np.copysign(1, alpha) yaw_setpoint = sign * min(self.max_thrust, abs(alpha)) # Command velocities self.motion_command(throttle_level, yaw_setpoint, 0.) # Publish feedback if counter % 10 == 0: self._as.publish_feedback(self._feedback) counter += 1 r.sleep() # Stop thruster self.motion_command(0., 0., 0.) rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result) def motion_command(self, throttle_level, thruster_angle, inclination_angle): incl = Float64() throttle = Float64() thrust = Float64() throttle.data = throttle_level thrust.data = thruster_angle incl.data = inclination_angle self.thruster_pub.publish(thrust) self.inclination_pub.publish(incl) self.throttle_pub.publish(throttle) def timer_callback(self, event): if self.nav_goal is None: #rospy.loginfo_throttle(30, "Nav goal is None!") return # Check if the goal has been reached try: (trans, rot) = self.listener.lookupTransform(self.nav_goal_frame, self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return start_pos = np.array(trans) end_pos = np.array([ self.nav_goal.position.x, self.nav_goal.position.y, self.nav_goal.position.z ]) rospy.logdebug("diff " + str(np.linalg.norm(start_pos - end_pos))) if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance: # Goal reached self.nav_goal = None def __init__(self, name): self._action_name = name self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.) self.max_throttle = rospy.get_param('~max_throttle', 2.) self.max_thrust = rospy.get_param('~max_thrust', 0.5) self.map_frame = rospy.get_param('~map_frame', 'map') self.base_frame = rospy.get_param('~base_frame', 'base_link') self.odom_frame = rospy.get_param('~odom_frame', 'odom') self.throttle_top = rospy.get_param('~throttle_cmd', '/throttle') self.thruster_top = rospy.get_param('~thruster_cmd', '/thruster') self.inclination_top = rospy.get_param('~inclination_cmd', '/inclination') self.as_name = rospy.get_param('~path_planner_as', 'path_planner') self.nav_goal = None self.listener = tf.TransformListener() rospy.Timer(rospy.Duration(2), self.timer_callback) self.throttle_pub = rospy.Publisher(self.throttle_top, Float64, queue_size=1) self.thruster_pub = rospy.Publisher(self.thruster_top, Float64, queue_size=1) self.inclination_pub = rospy.Publisher(self.inclination_top, Float64, queue_size=1) self._as = actionlib.SimpleActionServer(self.as_name, MoveBaseAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() rospy.loginfo("Announced action server with name: %s", self.as_name) rospy.spin()
class YawPlanner(object): # create messages that are used to publish feedback/result _feedback = MoveBaseFeedback() _result = MoveBaseResult() def execute_cb(self, goal): rospy.loginfo("Goal received") success = True self.nav_goal = goal.target_pose.pose self.nav_goal_frame = goal.target_pose.header.frame_id if self.nav_goal_frame is None or self.nav_goal_frame == '': rospy.logwarn("Goal has no frame id! Using utm by default") self.nav_goal_frame = '/utm' goal_point = PointStamped() goal_point.header.frame_id = self.nav_goal_frame goal_point.header.stamp = rospy.Time(0) goal_point.point.x = self.nav_goal.position.x goal_point.point.y = self.nav_goal.position.y goal_point.point.z = self.nav_goal.position.z try: goal_point_local = self.listener.transformPoint( self.nav_goal_frame, goal_point) self.nav_goal.position.x = goal_point_local.point.x self.nav_goal.position.y = goal_point_local.point.y self.nav_goal.position.z = goal_point_local.point.z except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print("Not transforming point to world local") pass rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x) r = rospy.Rate(11.) # 10hz counter = 0 while not rospy.is_shutdown() and self.nav_goal is not None: self.yaw_pid_enable.publish(True) # Preempted if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() success = False self.nav_goal = None # Stop thrusters rpm = ThrusterRPMs() rpm.thruster_1_rpm = 0. rpm.thruster_2_rpm = 0. self.rpm_pub.publish(rpm) self.yaw_pid_enable.publish(False) break # Publish feedback if counter % 10 == 0: try: (trans, rot) = self.listener.lookupTransform( self.nav_goal_frame, self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("Error with tf:" + str(self.nav_goal_frame) + " to " + str(self.base_frame)) continue pose_fb = PoseStamped() pose_fb.header.frame_id = self.nav_goal_frame pose_fb.pose.position.x = trans[0] pose_fb.pose.position.y = trans[1] pose_fb.pose.position.z = trans[2] self._feedback.base_position = pose_fb self._feedback.base_position.header.stamp = rospy.get_rostime() self._as.publish_feedback(self._feedback) #rospy.loginfo("Sending feedback") #Compute yaw setpoint. xdiff = self.nav_goal.position.x - pose_fb.pose.position.x ydiff = self.nav_goal.position.y - pose_fb.pose.position.y yaw_setpoint = math.atan2(ydiff, xdiff) self.yaw_pub.publish(yaw_setpoint) #rospy.loginfo("Yaw setpoint: %f", yaw_setpoint) # Thruster forward rpm = ThrusterRPMs() rpm.thruster_1_rpm = self.forward_rpm rpm.thruster_2_rpm = self.forward_rpm self.rpm_pub.publish(rpm) #rospy.loginfo("Thrusters forward") counter += 1 r.sleep() # Stop thruster rpm = ThrusterRPMs() rpm.thruster_1_rpm = 0.0 rpm.thruster_2_rpm = 0.0 self.rpm_pub.publish(rpm) #Stop yaw controller self.yaw_pid_enable.publish(False) rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result) def timer_callback(self, event): if self.nav_goal is None: #rospy.loginfo_throttle(30, "Nav goal is None!") return try: (trans, rot) = self.listener.lookupTransform(self.nav_goal_frame, self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return # TODO: we could use this code for the other check also goal_point = PointStamped() goal_point.header.frame_id = self.nav_goal_frame goal_point.header.stamp = rospy.Time(0) goal_point.point.x = self.nav_goal.position.x goal_point.point.y = self.nav_goal.position.y goal_point.point.z = self.nav_goal.position.z #print("Checking if nav goal is reached!") start_pos = np.array(trans) end_pos = np.array([ self.nav_goal.position.x, self.nav_goal.position.y, self.nav_goal.position.z ]) rospy.loginfo("diff " + str(np.linalg.norm(start_pos - end_pos))) if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance: rospy.loginfo("Reached goal!") self.nav_goal = None def __init__(self, name): """Publish yaw setpoints based on waypoints""" self._action_name = name #self.heading_offset = rospy.get_param('~heading_offsets', 5.) self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.) self.base_frame = rospy.get_param('~base_frame', "sam/base_link") self.utm_frame = rospy.get_param('~utm_frame', "utm") rpm_cmd_topic = rospy.get_param('~rpm_cmd_topic', '/sam/core/rpm_cmd') heading_setpoint_topic = rospy.get_param( '~heading_setpoint_topic', '/sam/ctrl/dynamic_heading/setpoint') yaw_pid_enable_topic = rospy.get_param( '~yaw_pid_enable_topic', '/sam/ctrl/dynamic_heading/pid_enable') self.forward_rpm = int(rospy.get_param('~forward_rpm', 700)) self.nav_goal = None self.listener = tf.TransformListener() rospy.Timer(rospy.Duration(2), self.timer_callback) self.rpm_pub = rospy.Publisher(rpm_cmd_topic, ThrusterRPMs, queue_size=10) self.yaw_pub = rospy.Publisher(heading_setpoint_topic, Float64, queue_size=10) self.yaw_pid_enable = rospy.Publisher(yaw_pid_enable_topic, Bool, queue_size=10) self._as = actionlib.SimpleActionServer(self._action_name, MoveBaseAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() rospy.loginfo("Announced action server with name: %s", self._action_name) rospy.spin()
def base_cb(self, goal): self.cb_executed = True result = MoveBaseResult() self.as_server.set_succeeded(result)
class BezierPlanner(object): # create messages that are used to publish feedback/result _feedback = MoveBaseFeedback() _result = MoveBaseResult() def execute_cb(self, goal): success = True self.nav_goal = goal.target_pose.pose # Target in word_utm coord, translate to world_local goal_point = PointStamped() goal_point.header.frame_id = "/world_utm" goal_point.header.stamp = rospy.Time(0) goal_point.point.x = self.nav_goal.position.x goal_point.point.y = self.nav_goal.position.y goal_point.point.z = self.nav_goal.position.z try: goal_point_local = self.listener.transformPoint( "/world_local", goal_point) self.nav_goal.position.x = goal_point_local.point.x self.nav_goal.position.y = goal_point_local.point.y self.nav_goal.position.z = goal_point_local.point.z except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print("Not transforming point to world local") pass rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x) r = rospy.Rate(10.) # 10hz counter = 0 while not rospy.is_shutdown() and self.nav_goal is not None: if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() success = False self.nav_goal = None break if counter % 100 == 0: path, pose = self.plan() self.pub.publish(path) self._feedback.base_position.header.stamp = rospy.get_rostime() self._feedback.base_position.header.frame_id = "/world_utm" self._feedback.base_position = pose self._as.publish_feedback(self._feedback) counter += 1 r.sleep() self.pub.publish(Path()) if success: #self._result.sequence = self._feedback.sequence rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result) def plan(self): try: (trans, rot) = self.listener.lookupTransform("/world_local", self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return Path(), PoseStamped() start_pos = np.array(trans) start_rot = np.array(rot) euler = tf.transformations.euler_from_quaternion(rot) start_pitch = euler[1] #np.radians(-40.0) # [rad] start_yaw = euler[2] # np.radians(180.0) # [rad] end_pos = np.array([ self.nav_goal.position.x, self.nav_goal.position.y, self.nav_goal.position.z ]) end_rot = [ self.nav_goal.orientation.x, self.nav_goal.orientation.y, self.nav_goal.orientation.z, self.nav_goal.orientation.w ] euler = tf.transformations.euler_from_quaternion(end_rot) end_pitch = euler[1] end_yaw = euler[2] curve, control_points = calc_4points_bezier_path( start_pos, start_yaw, start_pitch, end_pos, end_yaw, end_pitch, self.heading_offset, n_points=self.n_points) #derivatives_cp = bezier_derivatives_control_points(control_points, 1) path = Path() path.header.frame_id = "/world_local" path.header.stamp = rospy.get_rostime() for i in range(0, self.n_points): pose = PoseStamped() pose.pose.position.x = curve.T[0][i] pose.pose.position.y = curve.T[1][i] pose.pose.position.z = curve.T[2][i] path.poses.append(pose) # Revert pose to UTM coordinates for feedback to the BT pose = PoseStamped() pose.header.frame_id = "/world_local" pose.pose.position.x = start_pos[0] pose.pose.position.y = start_pos[1] pose.pose.position.z = start_pos[2] pose.pose.orientation.x = start_rot[0] pose.pose.orientation.y = start_rot[1] pose.pose.orientation.z = start_rot[2] pose.pose.orientation.w = start_rot[3] print("orientation") print(start_rot) try: pose_local = self.listener.transformPose("/world_utm", pose) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): pass return path, pose_local def timer_callback(self, event): if self.nav_goal is None: # print("Nav goal is None!") return try: (trans, rot) = self.listener.lookupTransform("/world", self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return goal_point = PointStamped() goal_point.header.frame_id = "/world" goal_point.header.stamp = rospy.Time(0) goal_point.point.x = self.nav_goal.position.x goal_point.point.y = self.nav_goal.position.y goal_point.point.z = self.nav_goal.position.z # try: # goal_point_base = self.listener.transformPoint(self.base_frame, goal_point) # if goal_point_base.point.x < 0.: # rospy.loginfo("Ahead of goal, returning success!") # self.nav_goal = None # return # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # pass #print("Checking if nav goal is reached!") start_pos = np.array(trans) end_pos = np.array([ self.nav_goal.position.x, self.nav_goal.position.y, self.nav_goal.position.z ]) if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance: rospy.loginfo("Reached goal!") self.nav_goal = None #else: # print("Did not reach nav goal!") def __init__(self, name): """Plot an example bezier curve.""" self._action_name = name self.heading_offset = rospy.get_param('~heading_offsets', 5.) self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.) self.n_points = rospy.get_param('~number_points', 100) self.base_frame = rospy.get_param('~base_frame', "base_link") self.nav_goal = None self.listener = tf.TransformListener() self.pub = rospy.Publisher('/global_plan', Path, queue_size=10) rospy.Timer(rospy.Duration(0.1), self.timer_callback) self._as = actionlib.SimpleActionServer(self._action_name, MoveBaseAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() rospy.loginfo("Announced action server with name: %s", self._action_name) r = rospy.Rate(10) # 10hz counter = 0 while not rospy.is_shutdown(): if counter % 100 == 0 and self.nav_goal is not None: path, pose = self.plan() self.pub.publish(path) r.sleep() counter += 1
class BezierPlanner(object): # create messages that are used to publish feedback/result _feedback = MoveBaseFeedback() _result = MoveBaseResult() def callback(self, pose_msg): self.nav_goal = pose_msg.pose path, pose = self.plan() self.pub.publish(path) def execute_cb(self, goal): # helper variables #r = rospy.Rate(1) success = True self.nav_goal = goal.target_pose.pose # append the seeds for the fibonacci sequence #self._feedback.base_position.header.frame_id = "/world" # publish info to the console for the user #rospy.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self._action_name, goal.order, self._feedback.sequence[0], self._feedback.sequence[1])) r = rospy.Rate(0.1) # 10hz while not rospy.is_shutdown() and self.nav_goal is not None: if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() success = False self.nav_goal = None break path, pose = self.plan() self.pub.publish(path) self._feedback.base_position = pose self._feedback.base_position.header.stamp = rospy.get_rostime() self._as.publish_feedback(self._feedback) r.sleep() self.pub.publish(Path()) if success: #self._result.sequence = self._feedback.sequence rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result) def plan(self): try: (trans, rot) = self.listener.lookupTransform("/world", self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return Path(), PoseStamped() start_pos = np.array(trans) euler = tf.transformations.euler_from_quaternion(rot) start_pitch = euler[1] #np.radians(-40.0) # [rad] start_yaw = euler[2] # np.radians(180.0) # [rad] end_pos = np.array( [self.nav_goal.position.x, self.nav_goal.position.y, -85.]) #if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance: # rospy.loginfo("Reached goal!") # self.nav_goal = None # return Path() end_rot = [ self.nav_goal.orientation.x, self.nav_goal.orientation.y, self.nav_goal.orientation.z, self.nav_goal.orientation.w ] euler = tf.transformations.euler_from_quaternion(end_rot) end_pitch = euler[1] end_yaw = euler[2] curve, control_points = calc_4points_bezier_path( start_pos, start_yaw, start_pitch, end_pos, end_yaw, end_pitch, self.heading_offset, n_points=self.n_points) #derivatives_cp = bezier_derivatives_control_points(control_points, 1) path = Path() path.header.frame_id = "/world" path.header.stamp = rospy.get_rostime() for i in range(0, self.n_points): pose = PoseStamped() pose.pose.position.x = curve.T[0][i] pose.pose.position.y = curve.T[1][i] pose.pose.position.z = curve.T[2][i] path.poses.append(pose) pose = PoseStamped() pose.pose.position.x = start_pos[0] pose.pose.position.y = start_pos[1] pose.pose.position.z = start_pos[2] return path, pose def timer_callback(self, event): if self.nav_goal is None: #print("Nav goal is None!") return try: (trans, rot) = self.listener.lookupTransform("/world", self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return #print("Checking if nav goal is reached!") start_pos = np.array(trans) end_pos = np.array( [self.nav_goal.position.x, self.nav_goal.position.y, -85.]) if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance: rospy.loginfo("Reached goal!") self.nav_goal = None #else: # print("Did not reach nav goal!") def __init__(self, name): """Plot an example bezier curve.""" self._action_name = name self.heading_offset = rospy.get_param('~heading_offsets', 5.) self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.) self.n_points = rospy.get_param('~number_points', 100) self.base_frame = rospy.get_param('~base_frame', "lolo_auv_1/base_link") self.nav_goal = None self.listener = tf.TransformListener() self.pub = rospy.Publisher('/global_plan', Path, queue_size=10) rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.callback) rospy.Timer(rospy.Duration(1), self.timer_callback) self._as = actionlib.SimpleActionServer(self._action_name, MoveBaseAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() rospy.loginfo("Announced action server with name: %s", self._action_name) r = rospy.Rate(0.1) # 10hz while not rospy.is_shutdown(): if self.nav_goal is not None: path, pose = self.plan() self.pub.publish(path) r.sleep()
This action server node will wait for 3 MoveBaseGoals and handle each differently: - succeed the first one - abort the 2nd goal - just ignore the 3rd one """ import rospy from scenario_test_tools.scriptable_move_base import ScriptableMoveBase from move_base_msgs.msg import MoveBaseAction, MoveBaseResult if __name__ == "__main__": rospy.init_node('move_base_fake') succeeded = MoveBaseResult() # By default, wait 5 secs for the robot to arrive result_delay = rospy.get_param("~result_delay", 5) # By default, wait forever for a move_base goal timeout = rospy.get_param("~timeout", None) # By default, publish transformation /map to /base_link pub_transform = rospy.get_param("~pub_transform", True) move_base = ScriptableMoveBase(rospy.get_name(), MoveBaseAction, default_result_delay=result_delay, pub_transform=pub_transform) move_base.start() rospy.loginfo("fake move base running")