def test_calculate_velocity( self): # only functions with 'test_'-prefix will be run! # Set up path pp = PurePursuit() pp.goal_margin = 0.1 # Set up test inputs goals = [ np.array([1., 0.]), np.array([1., 1.]), np.array([-0.5, -0.5]), np.array([0., -0.2]), np.array([0., 0.05]) ] # Desired outputs v_true = [0.22, 0.203703703704, -0.189655172414, 0.122222222222, 0.] w_true = [0., 0.203703703704, 0.379310344828, -1.22222222222, 0.] # Ensure that calculated outputs match desired outputs err_msg = "test goal ({},{}) has the velocity ({}, {}) instead of ({}, {})" for i in range(0, len(goals)): (v, w) = pp.calculate_velocity(goals[i]) self.assertTrue( np.abs(v - v_true[i]) < 1e-6 and np.abs(w - w_true[i]) < 1e-6, err_msg.format(goals[i][0], goals[i][1], v, w, v_true[i], w_true[i]))
def updatePath(): global x, y, theta, env, outImage, started, reprojMatrix, pp, transformMatrix, initFinished, goalPoint, goalChanged, following, newPath realTimeObs = True replanOnError = True while True: if initFinished and frame is not None: obstacles = getObstaclePositions(frame, transformMatrix) if realTimeObs or not started: env.setObstacles(np.squeeze(obstacles)) if not started: started = True if (realTimeObs and env.newObstacles()) or ( replanOnError and pp.highError) or goalChanged: goalChanged = False newPath = True try: env.generatePath(goalPoint) if env.splinePoints is None or len(env.splinePoints) == 0: pp = PurePursuit([(x, y)], LOOKAHEAD, 10000.0) else: pp = PurePursuit(env.splinePoints, LOOKAHEAD, 0.5) following = True started = True except: pass time.sleep(0.1)
def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) self.pursuit = PurePursuit() self.pursuit.set_look_ahead_distance(0.2) self.target_global = None self.listener = tf.TransformListener() self.pub_goal_marker = rospy.Publisher( "goal_marker", Marker, queue_size=1) self.pub_target_marker = rospy.Publisher( "target_marker", Marker, queue_size=1) self.pub_pid_goal = rospy.Publisher( "pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_box = rospy.Subscriber( "artifact_pose", PoseStamped, self.cb_pose, queue_size=1) self.sub_goal = rospy.Subscriber( "/move_base_simple/goal", PoseStamped, self.cb_goal, queue_size=1) self.timer = rospy.Timer(rospy.Duration(0.2), self.tracking)
def move_straight(self, start, end, reverse): dist_thresh = 0.03 dist = np.sqrt((start[1] - end[1])**2 + (start[0] - end[0])**2) k = 2 # timeout = 2 * dist / self.lin_vel_max timeout = 10 pp = PurePursuit(start, end) start_time = time.time() timer = time.time() while (abs(dist) > dist_thresh) and (time.time() - start_time < timeout): if (time.time() - timer) > self.dt: timer = time.time() dist = np.sqrt((self.state[1] - end[1])**2 + (self.state[0] - end[0])**2) v = min(self.lin_vel_max, dist * k) if reverse: v = v * -1 w = pp.get_ang_vel(self.state, v) self.send_vels(v, w) self.send_vels(0, 0)
def generate_pure_pursuit_path(): global pp pp = PurePursuit() for i in range(len(instructions)): # add x,y coords from each point in the generated trajectory as waypoints. # this is better than just adding the 5 nodes as waypoints. pp.add_point(instructions[i][1], instructions[i][2]) interpolate_path(i) #TODO comment out when not debugging.
def __init__(self): self.node_name = rospy.get_name() self.dis4constV = 3. # Distance for constant velocity self.pos_ctrl_max = 0.7 self.pos_ctrl_min = 0 self.ang_ctrl_max = 0.5 self.ang_ctrl_min = -0.5 self.pos_station_max = 0.5 self.pos_station_min = -0.5 self.cmd_ctrl_max = 0.7 self.cmd_ctrl_min = -0.7 self.station_keeping_dis = 0.5 # meters self.is_station_keeping = False self.start_navigation = False self.stop_pos = [] self.final_goal = None # The final goal that you want to arrive self.goal = self.final_goal self.robot_position = None rospy.loginfo("[%s] Initializing " % (self.node_name)) self.sub_goal = rospy.Subscriber("/path_points", PoseArray, self.path_cb, queue_size=1) #self.sub_goal = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=1) rospy.Subscriber('/odometry/ground_truth', Odometry, self.odom_cb, queue_size=1, buff_size=2**24) self.pub_cmd = rospy.Publisher("/X1/cmd_vel", Twist, queue_size=1) self.pub_lookahead = rospy.Publisher("/lookahead_point", Marker, queue_size=1) self.station_keeping_srv = rospy.Service("/station_keeping", SetBool, self.station_keeping_cb) self.navigate_srv = rospy.Service("/navigation", SetBool, self.navigation_cb) self.pos_control = PID_control("Position") self.ang_control = PID_control("Angular") self.ang_station_control = PID_control("Angular_station") self.pos_station_control = PID_control("Position_station") self.purepursuit = PurePursuit() self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position") self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular") self.pos_station_srv = Server(pos_PIDConfig, self.pos_station_pid_cb, "Angular_station") self.ang_station_srv = Server(ang_PIDConfig, self.ang_station_pid_cb, "Position_station") self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead") self.initialize_PID()
def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) ## parameters self.map_frame = rospy.get_param("~map_frame", 'odom') self.bot_frame = None self.switch_thred = 1 # change to next lane if reach next self.pursuit = PurePursuit() self.pursuit.set_look_ahead_distance(0.1) ## node path while not rospy.has_param("/guid_path"): rospy.loginfo("Wait for /guid_path") rospy.sleep(0.5) self.guid_path = rospy.get_param("/guid_path") self.last_node = -1 self.all_anchor_ids = rospy.get_param("/all_anchor_ids") self.joy_lock = False ## set first tracking lane self.set_lane(True) # variable self.target_global = None self.listener = tf.TransformListener() # markers self.pub_goal_marker = rospy.Publisher("goal_marker", Marker, queue_size=1) self.pub_target_marker = rospy.Publisher("target_marker", Marker, queue_size=1) # publisher, subscriber self.pub_pid_goal = rospy.Publisher("pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_box = rospy.Subscriber("anchor_pose", PoseDirectional, self.cb_goal, queue_size=1) sub_joy = rospy.Subscriber('joy_teleop/joy', Joy, self.cb_joy, queue_size=1) self.timer = rospy.Timer(rospy.Duration(0.1), self.tracking)
def __init__(self): self.node_name = rospy.get_name() self.dis4constV = 1. # Distance for constant velocity self.pos_ctrl_max = 1 self.pos_ctrl_min = 0.0 self.pos_station_max = 0.5 self.pos_station_min = -0.5 self.cmd_ctrl_max = 0.95 self.cmd_ctrl_min = -0.95 self.station_keeping_dis = 1 self.is_station_keeping = False self.start_navigation = False self.stop_pos = [] self.final_goal = None # The final goal that you want to arrive self.goal = self.final_goal self.robot_position = None #parameter self.sim = rospy.get_param('sim', False) rospy.loginfo("[%s] Initializing " %(self.node_name)) self.sub_goal = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=1) rospy.Subscriber('odometry', Odometry, self.odom_cb, queue_size = 1, buff_size = 2**24) if self.sim: from duckiepond_vehicle.msg import UsvDrive self.pub_cmd = rospy.Publisher("cmd_drive", UsvDrive, queue_size = 1) else : self.pub_cmd = rospy.Publisher("cmd_drive", MotorCmd, queue_size = 1) self.pub_lookahead = rospy.Publisher("lookahead_point", Marker, queue_size = 1) self.station_keeping_srv = rospy.Service("station_keeping", SetBool, self.station_keeping_cb) self.navigate_srv = rospy.Service("navigation", SetBool, self.navigation_cb) self.pos_control = PID_control("Position") self.ang_control = PID_control("Angular") self.ang_station_control = PID_control("Angular_station") self.pos_station_control = PID_control("Position_station") self.purepursuit = PurePursuit() self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position") self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular") self.pos_station_srv = Server(pos_PIDConfig, self.pos_station_pid_cb, "Angular_station") self.ang_station_srv = Server(ang_PIDConfig, self.ang_station_pid_cb, "Position_station") self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead") self.initialize_PID()
def __init__(self): self.node_name = rospy.get_name() self.station_keeping_dis = 1 self.is_station_keeping = False self.start_navigation = False self.stop_pos = [] self.goals = [] self.diving_points = [] self.diving_points_hold = [] self.get_path = False self.yaw = 0 self.dive = False self.finish_diving = True self.final_goal = None # The final goal that you want to arrive self.goal = self.final_goal self.robot_position = None self.dive_dis = 5 self.cycle = rospy.get_param("~cycle", True) rospy.loginfo("[%s] Initializing " % (self.node_name)) # self.pub_lookahead = rospy.Publisher("lookahead_point", Marker, queue_size = 1) self.pub_robot_goal = rospy.Publisher("robot_goal", RobotGoal, queue_size=1) self.path_srv = rospy.Service("set_path", SetRobotPath, self.path_cb) self.finish_diving_srv = rospy.Service("finish_diving", SetBool, self.finish_diving_cb) # self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead") self.purepursuit = PurePursuit() self.purepursuit.set_lookahead(5) self.odom_sub = rospy.Subscriber("odometry", Odometry, self.odom_cb, queue_size=1, buff_size=2**24) self.imu_sub = rospy.Subscriber("imu/data", Imu, self.imu_cb, queue_size=1, buff_size=2**24)
def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) self.end = np.array([[0.3, 2.2, 1.2, 2.2], [4.1, 3.9, 3, 3.9], [4.1, 2.2, 3, 2.2], [4.1, 0.5, 3, 0.5]]) self.pursuit = PurePursuit() self.pursuit.set_look_ahead_distance(0.2) self.pose = None self.pub_cmd = rospy.Publisher("cmd_vel", Twist, queue_size=1) self.pub_goal_marker = rospy.Publisher( "goal_marker", Marker, queue_size=1) self.pub_pid_goal = rospy.Publisher( "pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_pose = rospy.Subscriber( "pose", PoseStamped, self.cb_pose, queue_size=1) self.srv_topos = rospy.Service("to_position", GoToPos, self.to_pos)
def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) self.pursuit = PurePursuit() self.pursuit.set_look_ahead_distance(0.2) self.target_global = None self.listener = tf.TransformListener() # robot pose w.r.t the pose when plan pid_goal self.robot_now_pose = PoseStamped() self.pub_goal_marker = rospy.Publisher("goal_marker", Marker, queue_size=1) self.pub_target_marker = rospy.Publisher("target_marker", Marker, queue_size=1) self.pub_pid_goal = rospy.Publisher("pid_control/goal", PoseStamped, queue_size=1) self.pub_pid_pose = rospy.Publisher("pid_control/pose", PoseStamped, queue_size=1) # tracking robot pose before next self.tracking loop self.map_frame = rospy.get_param("~map_frame", 'map') self.robot_frame = rospy.get_param("~robot_frame", 'base_link') self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_box = rospy.Subscriber("anchor_position", PoseDirectional, self.cb_pose, queue_size=1) self.timer = rospy.Timer(rospy.Duration(0.2), self.tracking)
def generate_hard_path(): global pp pp = PurePursuit() # this creates a path manually set by changing waypoints here. # used for testing the robot on a small course by the lab. pp.add_point(0, -1) pp.add_point(2, -1) pp.add_point(2, 1) pp.add_point(0, 1)
def test_find_goal( self): # only functions with 'test_'-prefix will be run! # Set up path pp = PurePursuit() pp.path = Path() pose0 = PoseStamped() pose0.pose.position.x = 0.0 pose0.pose.position.y = 0.0 pp.path.poses.append(pose0) pose1 = PoseStamped() pose1.pose.position.x = 1.0 pose1.pose.position.y = 0.0 pp.path.poses.append(pose1) pose2 = PoseStamped() pose2.pose.position.x = 2.0 pose2.pose.position.y = 1.0 pp.path.poses.append(pose2) pp.lookahead = 1.0 # Set up test inputs x = [np.array([-0.25, 0.0]), np.array([0.5, 0.]), np.array([1.5, 0.5])] # Desired outputs goals_true = [ np.array([0.75, 0.]), np.array([1.41143782777, 0.411437827766]), np.array([2., 1.]) ] # Ensure that calculated outputs match desired outputs err_msg = "test point ({},{}) has the wrong goal ({}, {}) instead of ({}, {})" for i in range(0, len(x)): (pt, dist, seg) = pp.find_closest_point(x[i]) goal = pp.find_goal(x[i], pt, dist, seg) self.assertTrue( np.linalg.norm(goal - goals_true[i]) < 1e-6, err_msg.format(x[i][0], x[i][1], goal[0], goal[1], goals_true[i][0], goals_true[i][1]))
class Navigation(object): def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) self.pursuit = PurePursuit() self.pursuit.set_look_ahead_distance(0.2) self.target_global = None self.listener = tf.TransformListener() self.pub_goal_marker = rospy.Publisher( "goal_marker", Marker, queue_size=1) self.pub_target_marker = rospy.Publisher( "target_marker", Marker, queue_size=1) self.pub_pid_goal = rospy.Publisher( "pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_box = rospy.Subscriber( "artifact_pose", PoseStamped, self.cb_pose, queue_size=1) self.sub_goal = rospy.Subscriber( "/move_base_simple/goal", PoseStamped, self.cb_goal, queue_size=1) self.timer = rospy.Timer(rospy.Duration(0.2), self.tracking) def to_marker(self, goal, color=[0, 1, 0]): marker = Marker() marker.header.frame_id = goal.header.frame_id marker.header.stamp = rospy.Time.now() marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.orientation.w = 1 marker.pose.position.x = goal.pose.position.x marker.pose.position.y = goal.pose.position.y marker.id = 0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] return marker def transform_pose(self, pose, target_frame, source_frame): try: (trans_c, rot_c) = self.listener.lookupTransform( target_frame, source_frame, rospy.Time(0)) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logerr("faile to catch tf %s 2 %s" % (target_frame, source_frame)) return trans_mat = tf.transformations.translation_matrix(trans_c) rot_mat = tf.transformations.quaternion_matrix(rot_c) tran_mat = np.dot(trans_mat, rot_mat) target_mat = np.array([[1, 0, 0, pose.position.x], [0, 1, 0, pose.position.y], [0, 0, 1, pose.position.z], [0, 0, 0, 1]]) target = np.dot(tran_mat, target_mat) quat = tf.transformations.quaternion_from_matrix(target) trans = tf.transformations.translation_from_matrix(target) t_pose = PoseStamped() t_pose.header.frame_id = target_frame t_pose.pose.position.x = trans[0] t_pose.pose.position.y = trans[1] t_pose.pose.position.z = trans[2] t_pose.pose.orientation.x = quat[0] t_pose.pose.orientation.y = quat[1] t_pose.pose.orientation.z = quat[2] t_pose.pose.orientation.w = quat[3] return t_pose def tracking(self, event): if self.target_global is None: rospy.logerr("%s : no goal" % rospy.get_name()) return end_p = self.transform_pose( self.target_global.pose, "base_footprint", "map") self.pub_target_marker.publish(self.to_marker(end_p, [0, 0, 1])) start_p = PoseStamped() start_p.pose.position.x = 1 start_p.pose.position.y = 0 start_p.pose.position.z = 0 req_path = GetPlanRequest() req_path.start = start_p req_path.goal = end_p try: res_path = self.req_path_srv(req_path) except: rospy.logerr("%s : path request fail" % rospy.get_name()) return self.pursuit.set_path(res_path.plan) goal = self.pursuit.get_goal(start_p) if goal is None: rospy.logwarn("goal reached") return goal = self.transform_pose(goal.pose, "map", "base_footprint") self.pub_goal_marker.publish(self.to_marker(goal)) self.pub_pid_goal.publish(goal) def cb_pose(self, msg): for artifact in msg: if artifact.Class == "back_pack": self.target_global = self.transform_pose( artifact.pose, "map", "camera_color_optical_frame") def cb_goal(self, msg): self.target_global = msg
#!/usr/bin/env python import rospy from pure_pursuit import PurePursuit if __name__ == '__main__': rospy.init_node('pure_pursuit') pp = PurePursuit() rospy.spin()
class Navigation(object): def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) ## parameters self.map_frame = rospy.get_param("~map_frame", 'odom') self.bot_frame = 'base_link' self.switch_thred = 1.5 # change to next lane if reach next self.pursuit = PurePursuit() self.lka = rospy.get_param("~lookahead", 0.5) self.pursuit.set_look_ahead_distance(self.lka) ## node path while not rospy.has_param("/guid_path") and not rospy.is_shutdown(): rospy.loginfo("Wait for /guid_path") rospy.sleep(0.5) self.guid_path = rospy.get_param("/guid_path") self.tag_ang_init = rospy.get_param("/guid_path_ang_init") self.last_node = -1 self.next_node_id = None self.all_anchor_ids = rospy.get_param("/all_anchor_ids") self.joy_lock = False self.get_goal = True self.joint_ang = None ## set first tracking lane self.pub_robot_speech = rospy.Publisher("speech_case", String, queue_size=1) self.pub_robot_go = rospy.Publisher("robot_go", Bool, queue_size=1) rospy.sleep(1) # wait for the publisher to be set well self.set_lane(True) # variable self.target_global = None self.listener = tf.TransformListener() # markers self.pub_goal_marker = rospy.Publisher("goal_marker", Marker, queue_size=1) self.pub_target_marker = rospy.Publisher("target_marker", Marker, queue_size=1) # publisher, subscriber self.pub_pid_goal = rospy.Publisher("pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_box = rospy.Subscriber("anchor_position", PoseDirectional, self.cb_goal, queue_size=1) sub_joy = rospy.Subscriber('joy_teleop/joy', Joy, self.cb_joy, queue_size=1) sub_fr = rospy.Subscriber('front_right/ranges', DeviceRangeArray, self.cb_range, queue_size=1) sub_joint = rospy.Subscriber('/dynamixel_workbench/joint_states', JointState, self.cb_joint, queue_size=1) #Don't update goal too often self.last_update_goal = None self.goal_update_thred = 0.001 self.hist_goal = np.array([]) self.normal = True self.get_goal = True self.timer = rospy.Timer(rospy.Duration(0.1), self.tracking) # print("init done") # prevent sudden stop self.last_plan = None # keep searching until find path or reach search end self.search_angle = 10 * math.pi / 180 self.search_max = 5 ### stupid range drawing # for using tf to draw range br1 = tf2_ros.StaticTransformBroadcaster() br2 = tf2_ros.StaticTransformBroadcaster() # stf.header.frame_id = "uwb_back_left" # stf.child_frame_id = "base_link" # stf.transform.translation.x = -1*r_tag_points[0, 0] # stf.transform.translation.y = -1*r_tag_points[0, 1] # br1.sendTransform(stf) # stf2 = stf # stf2.header.stamp = rospy.Time.now() # stf2.header.frame_id = "uwb_front_right" # stf2.transform.translation.x = -1*r_tag_points[1, 0] # stf2.transform.translation.y = -1*r_tag_points[1, 1] # br2.sendTransform(stf2) stf = TransformStamped() stf.header.stamp = rospy.Time.now() stf.transform.rotation.w = 1 stf.header.frame_id = "base_link" stf.child_frame_id = "uwb_back_left" stf.transform.translation.x = r_tag_points[0, 0] stf.transform.translation.y = r_tag_points[0, 1] stf2 = TransformStamped() stf2.header.stamp = rospy.Time.now() stf2.transform.rotation.w = 1 stf2.header.frame_id = "base_link" stf2.child_frame_id = "uwb_front_right" stf2.transform.translation.x = r_tag_points[1, 0] stf2.transform.translation.y = r_tag_points[1, 1] # br2.sendTransform([stf, stf2]) # print(rospy.Time.now()) def tracking(self, event): if not self.normal: return st = rospy.Time.now() #rospy.loginfo("tracking loop") if self.target_global is None: rospy.logerr("%s : no goal" % rospy.get_name()) return else: #rospy.loginfo("%s :have seen goal" % rospy.get_name()) pass #print("fuckkckckc why???") #print(self.target_global) end_p = self.transform_pose(self.target_global.pose, self.bot_frame, self.map_frame) #end_p = self.target_global #end_p.header.frame_id = self.map_frame if end_p is None: return end_p.header.frame_id = self.bot_frame start_p = PoseStamped() start_p.pose.position.x = 0 start_p.pose.position.y = 0 start_p.pose.position.z = 0 start_p.header.frame_id = self.bot_frame #start_p = self.transform_pose(start_p.pose, self.map_frame, self.bot_frame) if start_p is None: return req_path = GetPlanRequest() req_path.start = start_p req_path.goal = end_p oe = end_p for i in range(self.search_max): try: res_path = self.req_path_srv(req_path) self.last_plan = res_path #rospy.loginfo("%s : plan success" % rospy.get_name()) break except: rospy.logerr("%s : path request fail" % rospy.get_name()) if self.last_plan is None: return res_path = self.last_plan r = np.linalg.norm([oe.pose.position.x, oe.pose.position.y]) theta = math.atan2(oe.pose.position.y, oe.pose.position.x) theta += (-1)**(i) * (i + 1) * self.search_angle end_p.pose.position.x = r * math.cos(theta) end_p.pose.position.y = r * math.sin(theta) self.pub_target_marker.publish(self.to_marker(end_p, [0, 0, 1])) self.pursuit.set_path(res_path.plan) goal = self.pursuit.get_goal(start_p) if goal is None: rospy.logwarn("goal reached, to next goal") self.target_global = None self.set_lane(True) return # print("trace", goal) goal = self.transform_pose(goal.pose, self.map_frame, self.bot_frame) goal.header.frame_id = self.map_frame self.pub_goal_marker.publish(self.to_marker(goal)) self.pub_pid_goal.publish(goal) et = rospy.Time.now() #print("Duration:", et.to_sec()-st.to_sec()) #print("============================================") def cb_goal(self, msg): if not self.get_goal: return self.bot_frame = msg.header.frame_id now_t = rospy.Time.now() if self.last_update_goal is None: self.target_global = msg.pose # posestamped self.target_global = self.transform_pose(msg.pose.pose, self.map_frame, msg.header.frame_id, msg.header.stamp) if self.target_global is None: return self.target_global.header.frame_id = self.map_frame return dt = now_t.to_sec() - self.last_update_goal.to_sec() if dt >= self.goal_update_thred: tg = self.transform_pose(msg.pose.pose, self.map_frame, msg.header.frame_id, msg.header.stamp) if tg is None: return tg.header.frame_id = self.map_frame self.hist_goal = np.append(self.hist_goal, tg) self.target_global = tg self.hist_goal = np.array([]) else: tg = self.transform_pose(msg.pose.pose, self.map_frame, msg.header.frame_id, msg.header.stamp) if tg is None: return tg.header.frame_id = self.map_frame self.hist_goal = np.append(self.hist_goal, tg) self.last_update_goal = now_t def cb_range(self, msg): if len(msg.rangeArray) == 0: return d = (msg.rangeArray[0].distance) / 1000. tid = msg.rangeArray[0].tag_id #print("d:", d) #print("tid:",tid) #print("next:",self.next_node_id) if tid != self.next_node_id: return #print("d:",d) #print("self.switch:",self.switch_thred) #print("") if d <= self.switch_thred: rospy.logwarn("goal reached, to next goal") self.target_global = None self.set_lane(True) return #print(self.target_global) def cb_joy(self, msg): #print("cb_joy") #switch = 0 #if msg.axes[-3] < -0.5: # switch = 1 #elif msg.axes[2] < -0.5: # switch = -1 #print("b:",msg.axes[-1]) #print("f:",msg.axes[-2]) switch = None if msg.buttons[-1] == 1 and msg.buttons[-2] == 0: switch = -1 elif msg.buttons[-1] == 0 and msg.buttons[-2] == 1: switch = 1 elif msg.buttons[-1] == 1 and msg.buttons[-2] == 1: return else: switch = 0 # switch = msg.axes[-1] if switch == 0: self.joy_lock = False if self.joy_lock: return if switch == 1: rospy.loginfo("Joy to the next lane.") self.set_lane(True) self.joy_lock = True elif switch == -1: rospy.loginfo("Joy to the last lane.") self.set_lane(False) self.joy_lock = True def cb_joint(self, msg): self.joint_ang = -1 * msg.position[0] def set_lane(self, next): self.pub_robot_go.publish(False) # to next node if next: self.last_node += 1 else: self.last_node -= 1 # select sound file s_anchor = str(self.guid_path[self.last_node]) if len(s_anchor) <= 9: sound_file1 = str(self.guid_path[self.last_node]) + '_1' sound_file2 = str(self.guid_path[self.last_node]) + '_2' else: sound_file1 = str(self.guid_path[self.last_node])[:-2] + '_3' sound_file2 = str(self.guid_path[self.last_node])[:-2] + '_4' ###### head and toe ##################### if self.last_node >= len(self.guid_path) - 1: rospy.loginfo("It's the last lane.") # play sound on robot self.pub_robot_sound(sound_file1) rospy.sleep(2) # this make anchorball speack # see pozyx_ros/src/multi_ranging_guid.py rospy.set_param("/guid_lane_next", 0) rospy.set_param("/velocity_mode", 0) self.last_node -= 1 return elif self.last_node < 0: rospy.loginfo("Back to first lane.") self.last_node = 0 ########################################## # play sound on robot self.pub_robot_sound(sound_file1) rospy.sleep(2) # set pozyx ranging tag id try: rospy.delete_param("/anchorballs") except KeyError: rospy.logerr("No such param /anchorballs") pozyx_id = {} pozyx_id[self.guid_path[self.last_node]] = self.all_anchor_ids[ self.guid_path[self.last_node]] pozyx_id[self.guid_path[self.last_node + 1]] = self.all_anchor_ids[ self.guid_path[self.last_node + 1]] rospy.set_param("/anchorballs", pozyx_id) # set last, next node (anchor) # this make anchorball speack # see pozyx_ros/src/multi_ranging_guid.py # next node id self.target_global = None self.next_node_id = self.all_anchor_ids[self.guid_path[self.last_node + 1]] rospy.set_param("/guid_lane_last", self.all_anchor_ids[self.guid_path[self.last_node]]) rospy.set_param( "/guid_lane_next", self.all_anchor_ids[self.guid_path[self.last_node + 1]]) # start turning (right, left or back) self.normal = False rospy.sleep(2) self.special_case() self.pub_robot_go.publish(True) # play sound on robot self.pub_robot_sound(sound_file2) rospy.sleep(5) # to wait for everything to reset # self.target_global = None def special_case(self): # we hard code turning situation here # not the bast solution, but an achievable one at the moment if self.last_node == 0: self.normal = True return this_ang = self.tag_ang_init[self.guid_path[self.last_node]] if this_ang != 180: self.get_goal = False self.target_global = None while self.target_global is None: self.target_global = PoseStamped() self.target_global.pose.position.x = 5 * math.cos( math.radians(this_ang)) self.target_global.pose.position.y = 5 * math.cos( math.radians(this_ang)) self.target_global = self.transform_pose( self.target_global.pose, self.map_frame, self.bot_frame) self.target_global.header.frame_id = self.map_frame self.normal = True self.pub_robot_go.publish(True) rospy.sleep(5) self.get_goal = True else: self.get_goal = False # 180 degree turn, stage 1, tie up harness while self.joint_ang > math.radians(5): # play sound on robot self.pub_robot_sound('tie_up') rospy.sleep(4) # 180 degree turn, stage 2, turn 180 turn_goal = None print("turn_goal") while turn_goal is None: turn_goal = PoseStamped() turn_goal.pose.position.x = -5 turn_goal_bot = turn_goal turn_goal_pid = turn_goal turn_goal_pid.pose.position.x = -0.05 turn_goal_pid.pose.position.y = -0.005 turn_goal = self.transform_pose(turn_goal.pose, self.map_frame, self.bot_frame) turn_goal_dummy = turn_goal self.pub_robot_go.publish(True) print("after") while math.fabs( math.atan2( turn_goal_bot.pose.position.y, turn_goal_bot.pose.position.x)) >= math.radians(90): print("pub turn goal") self.pub_pid_goal.publish( self.transform_pose(turn_goal_pid.pose, self.map_frame, self.bot_frame)) rospy.sleep(0.1) turn_goal_bot = self.transform_pose(turn_goal.pose, self.bot_frame, self.map_frame) print( math.fabs( math.atan2(turn_goal_bot.pose.position.y, turn_goal_bot.pose.position.x))) self.pub_robot_go.publish(False) # 180 degree turn, stage 3, release harness while self.joint_ang < math.radians(45): # play sound on robot self.pub_robot_sound('release') rospy.sleep(4) self.normal = True self.get_goal = True def pub_robot_sound(self, data): # play sound on robot str_msg = String() str_msg.data = data self.pub_robot_speech.publish(str_msg) def to_marker(self, goal, color=[0, 1, 0]): marker = Marker() marker.header.frame_id = goal.header.frame_id marker.header.stamp = rospy.Time.now() marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.orientation.w = 1 marker.pose.position.x = goal.pose.position.x marker.pose.position.y = goal.pose.position.y marker.id = 0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] return marker def transform_pose(self, pose, target_frame, source_frame, stamp=None): # print("Test rospy time 0", rospy.Time(0).to_sec()) # print("Test msg time", stamp.to_sec()) #print(target_frame) #print(source_frame) if target_frame is None or source_frame is None: return try: (trans_c, rot_c) = self.listener.lookupTransform(target_frame, source_frame, rospy.Time(0)) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("faile to catch tf %s 2 %s" % (target_frame, source_frame)) return trans_mat = tf.transformations.translation_matrix(trans_c) rot_mat = tf.transformations.quaternion_matrix(rot_c) tran_mat = np.dot(trans_mat, rot_mat) # print(trans_c) target_mat = np.array([[1, 0, 0, pose.position.x], [0, 1, 0, pose.position.y], [0, 0, 1, pose.position.z], [0, 0, 0, 1]]) target = np.dot(tran_mat, target_mat) quat = tf.transformations.quaternion_from_matrix(target) trans = tf.transformations.translation_from_matrix(target) t_pose = PoseStamped() t_pose.header.frame_id = target_frame t_pose.pose.position.x = trans[0] t_pose.pose.position.y = trans[1] t_pose.pose.position.z = trans[2] t_pose.pose.orientation.x = quat[0] t_pose.pose.orientation.y = quat[1] t_pose.pose.orientation.z = quat[2] t_pose.pose.orientation.w = quat[3] return t_pose
goalPoint = (-10000, -10000) goalChanged = False x = y = theta = None outImage = np.zeros((1, 1, 3), dtype=np.uint8) frame = None reprojMatrix = None transformMatrix = None started = False initFinished = False following = False newPath = False env = Environment(4.0) env.generatePath(goalPoint) pp = PurePursuit([(0.0, 0.0)], LOOKAHEAD, 0.5) lastReset = 0 def angleDiff(a, b): diff = abs(a - b) % math.pi if diff > math.pi: diff = 2.0 * math.pi - diff if (a - b >= 0 and a - b <= math.pi) or (a - b <= -math.pi and a - b >= -2.0 * math.pi): diff = 1 * diff else: diff = -1 * (math.pi - diff)
#!/usr/bin/env python3 import rospy import numpy as np from segway.msg import BaseCommand,Odometry,Path from pure_pursuit import PurePursuit from math import copysign from util import RunningAverage,clip pp=PurePursuit(.18,.18,.03)#.1588 is wheel separation def pathCB(msg): global pp pp.updatePath(msg) rospy.init_node("waypoint_following") rospy.Subscriber("waypoints",Path,pathCB,queue_size=1) def odomCB(msg): global pp pp.updateOdom(msg) rospy.Subscriber('odometry',Odometry,odomCB,queue_size=1) cmdpub=rospy.Publisher('target_vel',BaseCommand,queue_size=1) rate=rospy.Rate(30) stopped=False while not rospy.is_shutdown(): rate.sleep() v,w=pp.getControl(.3) c=BaseCommand() c.header.stamp=rospy.get_rostime() c.velocity=v c.omega=clip(w,-3,3) if not stopped: cmdpub.publish(c)
class Navigation(object): def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) self.end = np.array([[0.3, 2.2, 1.2, 2.2], [4.1, 3.9, 3, 3.9], [4.1, 2.2, 3, 2.2], [4.1, 0.5, 3, 0.5]]) self.pursuit = PurePursuit() self.pursuit.set_look_ahead_distance(0.2) self.pose = None self.pub_cmd = rospy.Publisher("cmd_vel", Twist, queue_size=1) self.pub_goal_marker = rospy.Publisher( "goal_marker", Marker, queue_size=1) self.pub_pid_goal = rospy.Publisher( "pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_pose = rospy.Subscriber( "pose", PoseStamped, self.cb_pose, queue_size=1) self.srv_topos = rospy.Service("to_position", GoToPos, self.to_pos) def pub_marker(self, goal=Marker()): marker = Marker() marker.header.frame_id = "global" marker.header.stamp = rospy.Time.now() marker.ns = "look ahead" marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.orientation.w = 1 marker.pose.position.x = goal.pose.position.x marker.pose.position.y = goal.pose.position.y marker.id = 0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.g = 1.0 self.pub_goal_marker.publish(marker) def to_pos(self, req): rospy.loginfo("%s : request pos %d" % (rospy.get_name(), req.pos)) res = GoToPosResponse() if req.pos < 0 or req.pos > 3: rospy.logerr("%s : pos not exist" % rospy.get_name()) res.result = False return res if self.pose is None: rospy.logerr("%s : no pose" % rospy.get_name()) res.result = False return res my_pose = np.array([self.pose.pose.position.x, self.pose.pose.position.y]) dis = np.linalg.norm(self.end[req.pos][:2]-my_pose) # turn arround -------------------------------------------------------- if dis > 0.5: while not rospy.is_shutdown(): quat = (self.pose.pose.orientation.x, self.pose.pose.orientation.y, self.pose.pose.orientation.z, self.pose.pose.orientation.w) _, _, yaw = tf.transformations.euler_from_quaternion(quat) ang_err = self.get_goal_angle(yaw, my_pose, self.end[req.pos][:2]) cmd_msg = Twist() if ang_err < -30: cmd_msg.linear.x = -0.15 cmd_msg.angular.z = -0.8 elif ang_err > 30: cmd_msg.linear.x = -0.15 cmd_msg.angular.z = 0.8 else: rospy.loginfo("start plan") break self.pub_cmd.publish(cmd_msg) rospy.sleep(0.1) rospy.sleep(1) # to pre pos -------------------------------------------------------- end_p = PoseStamped() end_p.pose.position.x = self.end[req.pos][0] end_p.pose.position.y = self.end[req.pos][1] end_p.pose.position.z = 0 while not rospy.is_shutdown(): req_path = GetPlanRequest() req_path.start = self.pose req_path.goal = end_p try: res_path = self.req_path_srv(req_path) except: rospy.logerr("%s : path request fail" % rospy.get_name()) res.result = False return res self.pursuit.set_path(res_path.plan) goal = self.pursuit.get_goal(self.pose) if goal is None: break self.pub_marker(goal) self.pub_pid_goal.publish(goal) rospy.sleep(0.1) # to target pos -------------------------------------------------------- # end_p = PoseStamped() # end_p.pose.position.x = self.end[req.pos][0] # end_p.pose.position.y = self.end[req.pos][1] # end_p.pose.position.z = 0 # # pre_p = PoseStamped() # # pre_p.pose.position.x = self.end[req.pos][2] # # pre_p.pose.position.y = self.end[req.pos][3] # # pre_p.pose.position.z = 0 # req_path = GetPlanRequest() # req_path.start = self.pose # req_path.goal = end_p # try: # res_path = self.req_path_srv(req_path) # except: # rospy.logerr("%s : path request fail" % rospy.get_name()) # res.result = False # return res # self.pursuit.set_path(res_path.plan) # while not rospy.is_shutdown(): # goal = self.pursuit.get_goal(self.pose) # if goal is None: # break # self.pub_marker(goal) # self.pub_pid_goal.publish(goal) # rospy.sleep(0.1) res.result = True return res def cb_pose(self, msg): self.pose = msg def get_goal_angle(self, robot_yaw, robot, goal): robot_angle = np.degrees(robot_yaw) p1 = [robot[0], robot[1]] p2 = [robot[0], robot[1]+1.] p3 = goal angle = self.get_angle(p1, p2, p3) result = angle - robot_angle result += 90 return result def get_angle(self, p1, p2, p3): v0 = np.array(p2) - np.array(p1) v1 = np.array(p3) - np.array(p1) angle = np.math.atan2(np.linalg.det([v0, v1]), np.dot(v0, v1)) return np.degrees(angle) # limit the angle to the range of [-180, 180] def angle_range(self, angle): if angle > 180: angle = angle - 360 angle = self.angle_range(angle) elif angle < -180: angle = angle + 360 angle = self.angle_range(angle) return angle
def __init__(self): self.node_name = rospy.get_name() self.state = "normal" self.station_keeping_dis = 1 self.is_station_keeping = False self.start_navigation = False self.over_bridge_count = 4 self.stop_pos = [] self.goals = [] self.full_goals = [] self.get_path = False self.final_goal = None # The final goal that you want to arrive self.goal = self.final_goal self.robot_position = None self.cycle = rospy.get_param("~cycle", True) self.gazebo = rospy.get_param("~gazebo", False) self.lookahead = rospy.get_param("~lookahead", 2.2) rospy.loginfo("LookAhead: " + str(self.lookahead)) self.over_bridge_counter = 0 self.satellite_list = [] self.satellite_thres = 15 self.imu_angle = 0 self.angle_thres = 0.85 self.pre_pose = [] self.bridge_mode = False self.stop_list = [] self.stop_start_timer = rospy.get_time() self.stop_end_timer = rospy.get_time() self.satellite_avg = 0 self.satellite_curr = 0 self.log_string = "" rospy.loginfo("[%s] Initializing " % (self.node_name)) # self.pub_lookahead = rospy.Publisher("lookahead_point", Marker, queue_size = 1) self.pub_robot_goal = rospy.Publisher("robot_goal", RobotGoal, queue_size=1) self.pub_fake_goal = rospy.Publisher("fake_goal", Marker, queue_size=1) self.pub_log_str = rospy.Publisher("log_str", String, queue_size=1) self.path_srv = rospy.Service("set_path", SetRobotPath, self.path_cb) self.reset_srv = rospy.Service("reset_goals", SetBool, self.reset_cb) # self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead") self.purepursuit = PurePursuit() self.purepursuit.set_lookahead(self.lookahead) rospy.Subscriber("odometry", Odometry, self.odom_cb, queue_size=1, buff_size=2**24) rospy.Subscriber("/mavros/global_position/raw/satellites", UInt32, self.satellite_cb, queue_size=1, buff_size=2**24)
class NAVIGATION(): def __init__(self): self.node_name = rospy.get_name() self.state = "normal" self.station_keeping_dis = 1 self.is_station_keeping = False self.start_navigation = False self.over_bridge_count = 4 self.stop_pos = [] self.goals = [] self.full_goals = [] self.get_path = False self.final_goal = None # The final goal that you want to arrive self.goal = self.final_goal self.robot_position = None self.cycle = rospy.get_param("~cycle", True) self.gazebo = rospy.get_param("~gazebo", False) self.lookahead = rospy.get_param("~lookahead", 2.2) rospy.loginfo("LookAhead: " + str(self.lookahead)) self.over_bridge_counter = 0 self.satellite_list = [] self.satellite_thres = 15 self.imu_angle = 0 self.angle_thres = 0.85 self.pre_pose = [] self.bridge_mode = False self.stop_list = [] self.stop_start_timer = rospy.get_time() self.stop_end_timer = rospy.get_time() self.satellite_avg = 0 self.satellite_curr = 0 self.log_string = "" rospy.loginfo("[%s] Initializing " % (self.node_name)) # self.pub_lookahead = rospy.Publisher("lookahead_point", Marker, queue_size = 1) self.pub_robot_goal = rospy.Publisher("robot_goal", RobotGoal, queue_size=1) self.pub_fake_goal = rospy.Publisher("fake_goal", Marker, queue_size=1) self.pub_log_str = rospy.Publisher("log_str", String, queue_size=1) self.path_srv = rospy.Service("set_path", SetRobotPath, self.path_cb) self.reset_srv = rospy.Service("reset_goals", SetBool, self.reset_cb) # self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead") self.purepursuit = PurePursuit() self.purepursuit.set_lookahead(self.lookahead) rospy.Subscriber("odometry", Odometry, self.odom_cb, queue_size=1, buff_size=2**24) rospy.Subscriber("/mavros/global_position/raw/satellites", UInt32, self.satellite_cb, queue_size=1, buff_size=2**24) # rospy.Subscriber("imu/data", Imu, self.imu_cb, queue_size = 1, buff_size = 2**24) def stop_state(self, time_threshold): if (rospy.get_time() - self.stop_start_timer) > time_threshold: self.state = "normal" return rg = RobotGoal() rg.goal.position.x, rg.goal.position.y = pursuit_point[ 0], pursuit_point[1] rg.robot = msg.pose.pose def imu_cb(self, msg): quat = (msg.orientation.x,\ msg.orientation.y,\ msg.orientation.z,\ msg.orientation.w) _, _, angle = tf.transformations.euler_from_quaternion(quat) while angle >= np.pi: angle = angle - 2 * np.pi while angle < -np.pi: angle = angle + 2 * np.pi self.imu_angle = angle def satellite_cb(self, msg): self.satellite_curr = msg.data def publish_fake_goal(self, x, y): marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.ns = "fake_goal" marker.type = marker.CUBE marker.action = marker.ADD marker.pose.position.x = x marker.pose.position.y = y marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 0 marker.pose.orientation.w = 1 marker.scale.x = 0.7 marker.scale.y = 0.7 marker.scale.z = 0.7 marker.color.a = 1.0 marker.color.b = 1.0 marker.color.g = 1.0 marker.color.r = 1.0 self.pub_fake_goal.publish(marker) def odom_cb(self, msg): self.robot_position = [ msg.pose.pose.position.x, msg.pose.pose.position.y ] if not self.is_station_keeping: self.stop_pos = [[ msg.pose.pose.position.x, msg.pose.pose.position.y ]] quat = (msg.pose.pose.orientation.x,\ msg.pose.pose.orientation.y,\ msg.pose.pose.orientation.z,\ msg.pose.pose.orientation.w) _, _, yaw = tf.transformations.euler_from_quaternion(quat) while yaw >= np.pi: yaw = yaw - 2 * np.pi while yaw < -np.pi: yaw = yaw + 2 * np.pi self.imu_angle = yaw if len( self.goals ) == 0 or not self.get_path: # if the robot haven't recieve any goal return reach_goal = self.purepursuit.set_robot_pose(self.robot_position, yaw) pursuit_point = self.purepursuit.get_pursuit_point() is_last_idx = self.purepursuit.is_last_idx() if reach_goal or pursuit_point is None or is_last_idx: if self.cycle: # The start point is the last point of the list self.stop_list = [] start_point = [ self.goals[-1].waypoint.position.x, self.goals[-1].waypoint.position.y ] self.full_goals[0] = self.full_goals[-1] self.purepursuit.set_goal(start_point, self.goals) else: rg = RobotGoal() rg.goal.position.x, rg.goal.position.y = self.goals[ -1].waypoint.position.x, self.goals[-1].waypoint.position.y rg.robot = msg.pose.pose rg.only_angle.data = False rg.mode.data = "normal" self.pub_robot_goal.publish(rg) return rg = RobotGoal() # if AUV is under the bridge if self.full_goals[self.purepursuit.current_waypoint_index - 1].bridge_start.data: self.bridge_mode = True rg.mode.data = "bridge" fake_goal, is_robot_over_goal = self.purepursuit.get_parallel_fake_goal( ) if fake_goal is None: return self.publish_fake_goal(fake_goal[0], fake_goal[1]) rg.goal.position.x, rg.goal.position.y = fake_goal[0], fake_goal[1] if is_robot_over_goal: if self.legal_angle(): if self.satellite_curr >= int( self.satellite_avg) or self.gazebo: self.over_bridge_counter = self.over_bridge_counter + 1 self.log_string = "over bridge, leagal angle, satellite" else: self.over_bridge_counter = 0 self.log_string = "over bridge, leagal angle, " + str( self.satellite_curr) + "," + str( self.satellite_avg) else: self.over_bridge_counter = 0 self.log_string = "over bridge, illeagal angle" else: self.over_bridge_counter = 0 self.log_string = "not over the bridge" if self.over_bridge_counter > self.over_bridge_count: if not (not self.cycle and self.purepursuit.current_waypoint_index == len(self.purepursuit.waypoints) - 1): rospy.loginfo("[%s]Arrived waypoint: %d" % ("Over Bridge", self.purepursuit.current_waypoint_index)) if self.purepursuit.status != -1: self.purepursuit.status = self.purepursuit.status + 1 self.purepursuit.current_waypoint_index = self.purepursuit.current_waypoint_index + 1 else: rg.mode.data = "normal" self.log_string = "not under bridge" self.bridge_mode = False rg.goal.position.x, rg.goal.position.y = pursuit_point[ 0], pursuit_point[1] if self.satellite_avg == 0: self.satellite_avg = self.satellite_curr else: self.satellite_avg = (self.satellite_avg * 3. + self.satellite_curr) / 4. if self.full_goals[self.purepursuit.current_waypoint_index - 1].stop_time.data != 0: if self.purepursuit.current_waypoint_index not in self.stop_list: self.stop_list.append(self.purepursuit.current_waypoint_index) self.state = "stop" self.stop_start_timer = rospy.get_time() if self.state == "stop": time_threshold = self.full_goals[ self.purepursuit.current_waypoint_index - 1].stop_time.data if (rospy.get_time() - self.stop_start_timer) > time_threshold: self.state = "normal" else: rg.goal.position.x, rg.goal.position.y = self.robot_position[ 0], self.robot_position[1] rg.robot = msg.pose.pose self.purepursuit.bridge_mode = self.bridge_mode self.pub_robot_goal.publish(rg) self.pre_pose = [msg.pose.pose.position.x, msg.pose.pose.position.y] #yaw = yaw + np.pi/2. # if reach_goal or reach_goal is None: # self.publish_lookahead(self.robot_position, self.final_goal[-1]) # else: # self.publish_lookahead(self.robot_position, pursuit_point) ss = String() ss.data = self.log_string self.pub_log_str.publish(ss) def legal_angle(self): if self.pre_pose != []: angle = self.getAngle() if angle < self.angle_thres: return True return False # Calculate the angle difference between robot heading and vector start from start_pose, end at end_pose and unit x vector of odom frame, # in radian def getAngle(self): if self.pre_pose == []: return delta_x = self.robot_position[0] - self.pre_pose[0] delta_y = self.robot_position[1] - self.pre_pose[1] theta = np.arctan2(delta_y, delta_x) angle = theta - self.imu_angle # Normalize in [-pi, pi) while angle >= np.pi: angle = angle - 2 * np.pi while angle < -np.pi: angle = angle + 2 * np.pi # print(theta, self.imu_angle, abs(angle)) return abs(angle) def reset_cb(self, req): if req.data == True: self.full_goals = [] self.goals = [] self.get_path = False res = SetBoolResponse() res.success = True res.message = "recieved" return res def path_cb(self, req): rospy.loginfo("Get Path") res = SetRobotPathResponse() wp = WayPoint() wp.bridge_start.data = False wp.bridge_end.data = False self.full_goals.append(wp) if len(req.data.list) > 0: self.goals = req.data.list self.full_goals = self.full_goals + req.data.list self.get_path = True self.purepursuit.set_goal(self.robot_position, self.goals) res.success = True return res def get_goal_angle(self, robot_yaw, robot, goal): robot_angle = np.degrees(robot_yaw) p1 = [robot[0], robot[1]] p2 = [robot[0], robot[1] + 1.] p3 = goal angle = self.get_angle(p1, p2, p3) result = angle - robot_angle result = self.angle_range(-(result + 90.)) return result def get_angle(self, p1, p2, p3): v0 = np.array(p2) - np.array(p1) v1 = np.array(p3) - np.array(p1) angle = np.math.atan2(np.linalg.det([v0, v1]), np.dot(v0, v1)) return np.degrees(angle) def angle_range(self, angle): # limit the angle to the range of [-180, 180] if angle > 180: angle = angle - 360 angle = self.angle_range(angle) elif angle < -180: angle = angle + 360 angle = self.angle_range(angle) return angle def get_distance(self, p1, p2): return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2) def publish_lookahead(self, robot, lookahead): marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.ns = "pure_pursuit" marker.type = marker.LINE_STRIP marker.action = marker.ADD wp = Point() wp.x, wp.y = robot[:2] wp.z = 0 marker.points.append(wp) wp = Point() wp.x, wp.y = lookahead[0], lookahead[1] wp.z = 0 marker.points.append(wp) marker.id = 0 marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 0.5 marker.color.a = 1.0 marker.color.b = 1.0 marker.color.g = 1.0 marker.color.r = 0.0 #self.pub_lookahead.publish(marker) def lookahead_cb(self, config, level): print("Look Ahead Distance: {Look_Ahead}\n".format(**config)) lh = float("{Look_Ahead}".format(**config)) self.purepursuit.set_lookahead(lh) return config
import math import numpy as np from environment import Environment, Spline from pure_pursuit import PurePursuit from obstacleDetector import getObstaclePositions from imgUtils import overlayImages x = y = theta = None cte = None env = Environment(3.0) #path = [(x, 0.0) for x in np.linspace(-10, 10, 200)] #env.waypoints = [(-10, 0), (10, 0)] #env.splinePoints = path # TODO remove env.generatePath((-18.0, -6.0)) pp = PurePursuit([(0.0, 0.0)], 2.5, 0.5) def updateRobotPos(): global cte, x, y, theta, env frame = cv2.imread("ObstacleNN/TrainingData1/image183.png") print("Setting up...") setupImgs = [] for i in range(15): setupImgs.append(frame) time.sleep(0.1) transformMatrix, reprojMatrix = setup(setupImgs)
import time from final_base import start, end, tankDrive, turn, forward, getFloor, getProximity, stop, beepSync, setMusicNote, updateImage, updateImage2 import math import numpy as np from environment import Environment, Spline from pure_pursuit import PurePursuit x = y = theta = None cte = None env = Environment() #path = [(x, 0.0) for x in np.linspace(-10, 10, 200)] #env.waypoints = [(-10, 0), (10, 0)] #env.splinePoints = path # TODO remove env.generatePath(None) pp = PurePursuit(env.splinePoints, 2.5, 0.5) def updateRobotPos(): global cte, x, y, theta, env cap = WebcamVideoStream(src=int(sys.argv[1])) cap.start() print("Setting up...") setupImgs = [] for i in range(15): frame = cap.read() setupImgs.append(frame) time.sleep(0.1)
class Robot_PID(): def __init__(self): self.node_name = rospy.get_name() self.dis4constV = 1. # Distance for constant velocity self.pos_ctrl_max = 1 self.pos_ctrl_min = 0.0 self.pos_station_max = 0.5 self.pos_station_min = -0.5 self.cmd_ctrl_max = 0.95 self.cmd_ctrl_min = -0.95 self.station_keeping_dis = 1 self.is_station_keeping = False self.start_navigation = False self.stop_pos = [] self.final_goal = None # The final goal that you want to arrive self.goal = self.final_goal self.robot_position = None #parameter self.sim = rospy.get_param('sim', False) rospy.loginfo("[%s] Initializing " %(self.node_name)) self.sub_goal = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=1) rospy.Subscriber('odometry', Odometry, self.odom_cb, queue_size = 1, buff_size = 2**24) if self.sim: from duckiepond_vehicle.msg import UsvDrive self.pub_cmd = rospy.Publisher("cmd_drive", UsvDrive, queue_size = 1) else : self.pub_cmd = rospy.Publisher("cmd_drive", MotorCmd, queue_size = 1) self.pub_lookahead = rospy.Publisher("lookahead_point", Marker, queue_size = 1) self.station_keeping_srv = rospy.Service("station_keeping", SetBool, self.station_keeping_cb) self.navigate_srv = rospy.Service("navigation", SetBool, self.navigation_cb) self.pos_control = PID_control("Position") self.ang_control = PID_control("Angular") self.ang_station_control = PID_control("Angular_station") self.pos_station_control = PID_control("Position_station") self.purepursuit = PurePursuit() self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position") self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular") self.pos_station_srv = Server(pos_PIDConfig, self.pos_station_pid_cb, "Angular_station") self.ang_station_srv = Server(ang_PIDConfig, self.ang_station_pid_cb, "Position_station") self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead") self.initialize_PID() def odom_cb(self, msg): robot_position = [msg.pose.pose.position.x, msg.pose.pose.position.y] if not self.is_station_keeping: self.stop_pos = [[msg.pose.pose.position.x, msg.pose.pose.position.y]] quat = (msg.pose.pose.orientation.x,\ msg.pose.pose.orientation.y,\ msg.pose.pose.orientation.z,\ msg.pose.pose.orientation.w) _, _, yaw = tf.transformations.euler_from_quaternion(quat) self.robot_position = robot_position if self.goal is None: # if the robot haven't recieve any goal return if not self.start_navigation: return reach_goal = self.purepursuit.set_robot_pose(robot_position, yaw) pursuit_point = self.purepursuit.get_pursuit_point() #yaw = yaw + np.pi/2. if reach_goal or reach_goal is None: self.publish_lookahead(robot_position, self.final_goal[-1]) goal_distance = self.get_distance(robot_position, self.final_goal[-1]) goal_angle = self.get_goal_angle(yaw, robot_position, self.final_goal[-1]) pos_output, ang_output = self.station_keeping(goal_distance, goal_angle) elif self.is_station_keeping: rospy.loginfo("Station Keeping") self.publish_lookahead(robot_position, self.goal[0]) goal_distance = self.get_distance(robot_position, self.goal[0]) goal_angle = self.get_goal_angle(yaw, robot_position, self.goal[0]) pos_output, ang_output = self.station_keeping(goal_distance, goal_angle) else: self.publish_lookahead(robot_position, pursuit_point) goal_distance = self.get_distance(robot_position, pursuit_point) goal_angle = self.get_goal_angle(yaw, robot_position, pursuit_point) pos_output, ang_output = self.control(goal_distance, goal_angle) if self.sim: cmd_msg = UsvDrive() else: cmd_msg = MotorCmd() cmd_msg.left = self.cmd_constarin(pos_output - ang_output) cmd_msg.right = self.cmd_constarin(pos_output + ang_output) self.pub_cmd.publish(cmd_msg) def control(self, goal_distance, goal_angle): self.pos_control.update(goal_distance) self.ang_control.update(goal_angle) # pos_output will always be positive pos_output = self.pos_constrain(-self.pos_control.output/self.dis4constV) # -1 = -180/180 < output/180 < 180/180 = 1 ang_output = self.ang_control.output/180. return pos_output, ang_output def station_keeping(self, goal_distance, goal_angle): self.pos_station_control.update(goal_distance) self.ang_station_control.update(goal_angle) # pos_output will always be positive pos_output = self.pos_station_constrain(-self.pos_station_control.output/self.dis4constV) # -1 = -180/180 < output/180 < 180/180 = 1 ang_output = self.ang_station_control.output/180. # if the goal is behind the robot if abs(goal_angle) > 90: pos_output = - pos_output ang_output = - ang_output return pos_output, ang_output def goal_cb(self, p): if self.final_goal is None: self.final_goal = [] self.final_goal.append([p.pose.position.x, p.pose.position.y]) self.goal = self.final_goal def station_keeping_cb(self, req): if req.data == True: self.goal = self.stop_pos self.is_station_keeping = True else: self.is_station_keeping = False res = SetBoolResponse() res.success = True res.message = "recieved" return res def navigation_cb(self, req): if req.data == True: if not self.is_station_keeping: self.purepursuit.set_goal(self.robot_position, self.goal) self.is_station_keeping = False self.start_navigation = True else: self.start_navigation = False self.final_goal = None self.goal = self.stop_pos res = SetBoolResponse() res.success = True res.message = "recieved" return res def cmd_constarin(self, input): if input > self.cmd_ctrl_max: return self.cmd_ctrl_max if input < self.cmd_ctrl_min: return self.cmd_ctrl_min return input def pos_constrain(self, input): if input > self.pos_ctrl_max: return self.pos_ctrl_max if input < self.pos_ctrl_min: return self.pos_ctrl_min return input def pos_station_constrain(self, input): if input > self.pos_station_max: return self.pos_station_max if input < self.pos_station_min: return self.pos_station_min return input def initialize_PID(self): self.pos_control.setSampleTime(1) self.ang_control.setSampleTime(1) self.pos_station_control.setSampleTime(1) self.ang_station_control.setSampleTime(1) self.pos_control.SetPoint = 0.0 self.ang_control.SetPoint = 0.0 self.pos_station_control.SetPoint = 0.0 self.ang_station_control.SetPoint = 0.0 def get_goal_angle(self, robot_yaw, robot, goal): robot_angle = np.degrees(robot_yaw) p1 = [robot[0], robot[1]] p2 = [robot[0], robot[1]+1.] p3 = goal angle = self.get_angle(p1, p2, p3) result = angle - robot_angle result = self.angle_range(-(result + 90.)) return result def get_angle(self, p1, p2, p3): v0 = np.array(p2) - np.array(p1) v1 = np.array(p3) - np.array(p1) angle = np.math.atan2(np.linalg.det([v0,v1]),np.dot(v0,v1)) return np.degrees(angle) def angle_range(self, angle): # limit the angle to the range of [-180, 180] if angle > 180: angle = angle - 360 angle = self.angle_range(angle) elif angle < -180: angle = angle + 360 angle = self.angle_range(angle) return angle def get_distance(self, p1, p2): return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2) def pos_pid_cb(self, config, level): print("Position: [Kp]: {Kp} [Ki]: {Ki} [Kd]: {Kd}\n".format(**config)) Kp = float("{Kp}".format(**config)) Ki = float("{Ki}".format(**config)) Kd = float("{Kd}".format(**config)) self.pos_control.setKp(Kp) self.pos_control.setKi(Ki) self.pos_control.setKd(Kd) return config def publish_lookahead(self, robot, lookahead): marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.ns = "pure_pursuit" marker.type = marker.LINE_STRIP marker.action = marker.ADD wp = Point() wp.x, wp.y = robot[:2] wp.z = 0 marker.points.append(wp) wp = Point() wp.x, wp.y = lookahead[0], lookahead[1] wp.z = 0 marker.points.append(wp) marker.id = 0 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 1.0 marker.color.b = 1.0 marker.color.g = 1.0 self.pub_lookahead.publish(marker) def ang_pid_cb(self, config, level): print("Angular: [Kp]: {Kp} [Ki]: {Ki} [Kd]: {Kd}\n".format(**config)) Kp = float("{Kp}".format(**config)) Ki = float("{Ki}".format(**config)) Kd = float("{Kd}".format(**config)) self.ang_control.setKp(Kp) self.ang_control.setKi(Ki) self.ang_control.setKd(Kd) return config def pos_station_pid_cb(self, config, level): print("Position: [Kp]: {Kp} [Ki]: {Ki} [Kd]: {Kd}\n".format(**config)) Kp = float("{Kp}".format(**config)) Ki = float("{Ki}".format(**config)) Kd = float("{Kd}".format(**config)) self.pos_station_control.setKp(Kp) self.pos_station_control.setKi(Ki) self.pos_station_control.setKd(Kd) return config def ang_station_pid_cb(self, config, level): print("Angular: [Kp]: {Kp} [Ki]: {Ki} [Kd]: {Kd}\n".format(**config)) Kp = float("{Kp}".format(**config)) Ki = float("{Ki}".format(**config)) Kd = float("{Kd}".format(**config)) self.ang_station_control.setKp(Kp) self.ang_station_control.setKi(Ki) self.ang_station_control.setKd(Kd) return config def lookahead_cb(self, config, level): print("Look Ahead Distance: {Look_Ahead}\n".format(**config)) lh = float("{Look_Ahead}".format(**config)) self.purepursuit.set_lookahead(lh) return config
class NAVIGATION(): def __init__(self): self.node_name = rospy.get_name() self.station_keeping_dis = 1 self.is_station_keeping = False self.start_navigation = False self.stop_pos = [] self.goals = [] self.diving_points = [] self.diving_points_hold = [] self.get_path = False self.yaw = 0 self.dive = False self.finish_diving = True self.final_goal = None # The final goal that you want to arrive self.goal = self.final_goal self.robot_position = None self.dive_dis = 5 self.cycle = rospy.get_param("~cycle", True) rospy.loginfo("[%s] Initializing " % (self.node_name)) # self.pub_lookahead = rospy.Publisher("lookahead_point", Marker, queue_size = 1) self.pub_robot_goal = rospy.Publisher("robot_goal", RobotGoal, queue_size=1) self.path_srv = rospy.Service("set_path", SetRobotPath, self.path_cb) self.finish_diving_srv = rospy.Service("finish_diving", SetBool, self.finish_diving_cb) # self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead") self.purepursuit = PurePursuit() self.purepursuit.set_lookahead(5) self.odom_sub = rospy.Subscriber("odometry", Odometry, self.odom_cb, queue_size=1, buff_size=2**24) self.imu_sub = rospy.Subscriber("imu/data", Imu, self.imu_cb, queue_size=1, buff_size=2**24) def odom_cb(self, msg): if self.dive and not self.finish_diving: return self.robot_position = [ msg.pose.pose.position.x, msg.pose.pose.position.y ] if not self.is_station_keeping: self.stop_pos = [[ msg.pose.pose.position.x, msg.pose.pose.position.y ]] quat = (msg.pose.pose.orientation.x,\ msg.pose.pose.orientation.y,\ msg.pose.pose.orientation.z,\ msg.pose.pose.orientation.w) _, _, yaw = tf.transformations.euler_from_quaternion(quat) if len( self.goals ) == 0 or not self.get_path: # if the robot haven't recieve any goal return reach_goal = self.purepursuit.set_robot_pose(self.robot_position, yaw) pursuit_point = self.purepursuit.get_pursuit_point() is_last_idx = self.purepursuit.is_last_idx() if reach_goal or pursuit_point is None or is_last_idx: if self.cycle: # The start point is the last point of the list start_point = [ self.goals[-1].position.x, self.goals[-1].position.y ] self.purepursuit.set_goal(start_point, self.goals) self.diving_points = self.diving_points_hold[:] else: rg = RobotGoal() rg.goal.position.x, rg.goal.position.y = self.goals[ -1].position.x, self.goals[-1].position.y rg.robot = msg.pose.pose self.pub_robot_goal.publish(rg) return self.dive = self.if_dive() rg = RobotGoal() rg.goal.position.x, rg.goal.position.y = pursuit_point[ 0], pursuit_point[1] rg.robot = msg.pose.pose rg.only_angle.data = False self.pub_robot_goal.publish(rg) #yaw = yaw + np.pi/2. # if reach_goal or reach_goal is None: # self.publish_lookahead(self.robot_position, self.final_goal[-1]) # else: # self.publish_lookahead(self.robot_position, pursuit_point) def imu_cb(self, msg): quat = (msg.orientation.x,\ msg.orientation.y,\ msg.orientation.z,\ msg.orientation.w) _, _, yaw = tf.transformations.euler_from_quaternion(quat) self.yaw = yaw if self.dive: if self.finish_diving: self.dive = False reach_goal = self.purepursuit.set_robot_pose( self.robot_position, yaw) pursuit_point = self.purepursuit.get_pursuit_point() is_last_idx = self.purepursuit.is_last_idx() if reach_goal or pursuit_point is None or is_last_idx: rg = RobotGoal() rg.goal.position.x, rg.goal.position.y = self.goals[ -1].position.x, self.goals[-1].position.y rg.robot = msg.pose.pose self.pub_robot_goal.publish(rg) return rg = RobotGoal() rg.goal.position.x, rg.goal.position.y = pursuit_point[ 0], pursuit_point[1] p = Pose() p.position.x = self.robot_position[0] p.position.y = self.robot_position[1] rg.robot = p rg.only_angle.data = True self.pub_robot_goal.publish(rg) def path_cb(self, req): rospy.loginfo("Get Path") res = SetRobotPathResponse() if len(req.data.list) > 0: self.goals = req.data.list self.diving_points_hold = self.goals[:] self.diving_points = self.diving_points_hold[:] self.get_path = True self.purepursuit.set_goal(self.robot_position, self.goals) res.success = True return res def finish_diving_cb(self, req): if req.data == True: rospy.loginfo("Finish Diving") self.finish_diving = True res = SetBoolResponse() res.success = True res.message = "recieved" return res def if_dive(self): arr = False del_pt = None for dv_pt in self.diving_points: p1 = [dv_pt.position.x, dv_pt.position.y] p2 = self.robot_position if self.get_distance(p1, p2) <= self.dive_dis: print("DIVE") arr = True del_pt = dv_pt self.finish_diving = False self.srv_dive() if arr: self.diving_points.remove(del_pt) return True return False def srv_dive(self): #rospy.wait_for_service('/set_path') rospy.loginfo("SRV: Send diving") set_bool = SetBoolRequest() set_bool.data = True try: srv = rospy.ServiceProxy('dive', SetBool) resp = srv(set_bool) return resp except rospy.ServiceException, e: print "Service call failed: %s" % e
class Robot_PID(): def __init__(self): self.node_name = rospy.get_name() self.dis4constV = 3. # Distance for constant velocity self.pos_ctrl_max = 0.7 self.pos_ctrl_min = 0 self.ang_ctrl_max = 1.0 self.ang_ctrl_min = -1.0 self.pos_station_max = 0.5 self.pos_station_min = -0.5 self.turn_threshold = 20 self.cmd_ctrl_max = 0.7 self.cmd_ctrl_min = -0.7 self.station_keeping_dis = 0.5 # meters self.frame_id = 'map' self.is_station_keeping = False self.stop_pos = [] self.final_goal = None # The final goal that you want to arrive self.goal = self.final_goal self.robot_position = None rospy.loginfo("[%s] Initializing " % (self.node_name)) self.sub_goal = rospy.Subscriber("/planning_path", Path, self.path_cb, queue_size=1) #self.sub_goal = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=1) rospy.Subscriber('/odometry/ground_truth', Odometry, self.odom_cb, queue_size=1, buff_size=2**24) self.pub_cmd = rospy.Publisher("/X1/cmd_vel", Twist, queue_size=1) self.pub_lookahead = rospy.Publisher("/lookahead_point", Marker, queue_size=1) self.pub_goal = rospy.Publisher("/goal_point", Marker, queue_size=1) self.station_keeping_srv = rospy.Service("/station_keeping", SetBool, self.station_keeping_cb) self.pos_control = PID_control("Position") self.ang_control = PID_control("Angular") self.ang_station_control = PID_control("Angular_station") self.pos_station_control = PID_control("Position_station") self.purepursuit = PurePursuit() self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position") self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular") self.pos_station_srv = Server(pos_PIDConfig, self.pos_station_pid_cb, "Angular_station") self.ang_station_srv = Server(ang_PIDConfig, self.ang_station_pid_cb, "Position_station") self.initialize_PID() def path_cb(self, msg): self.final_goal = [] for pose in msg.poses: self.final_goal.append( [pose.pose.position.x, pose.pose.position.y]) self.goal = self.final_goal self.is_station_keeping = False self.start_navigation = True self.purepursuit.set_goal(self.robot_position, self.goal) def odom_cb(self, msg): self.frame_id = msg.header.frame_id self.robot_position = [ msg.pose.pose.position.x, msg.pose.pose.position.y ] if not self.is_station_keeping: self.stop_pos = [ msg.pose.pose.position.x, msg.pose.pose.position.y ] quat = (msg.pose.pose.orientation.x,\ msg.pose.pose.orientation.y,\ msg.pose.pose.orientation.z,\ msg.pose.pose.orientation.w) _, _, yaw = tf.transformations.euler_from_quaternion(quat) if self.goal is None: # if the robot haven't recieve any goal return reach_goal = self.purepursuit.set_robot_pose(self.robot_position, yaw) pursuit_point = self.purepursuit.get_pursuit_point() if reach_goal or reach_goal is None: pos_output, ang_output = 0, 0 else: self.publish_lookahead(self.robot_position, pursuit_point) goal_distance = self.get_distance(self.robot_position, pursuit_point) goal_angle = self.get_goal_angle(yaw, self.robot_position, pursuit_point) pos_output, ang_output = self.control(goal_distance, goal_angle) #yaw = yaw + np.pi/2 '''goal_distance = self.get_distance(robot_position, self.goal) goal_angle = self.get_goal_angle(yaw, robot_position, self.goal) if goal_distance < self.station_keeping_dis or self.is_station_keeping: rospy.loginfo("Station Keeping") pos_output, ang_output = self.station_keeping(goal_distance, goal_angle) else: pos_output, ang_output = self.control(goal_distance, goal_angle)''' cmd_msg = Twist() cmd_msg.linear.x = pos_output cmd_msg.angular.z = ang_output self.pub_cmd.publish(cmd_msg) #self.publish_goal(self.goal) def control(self, goal_distance, goal_angle): self.pos_control.update(goal_distance) self.ang_control.update(goal_angle) # pos_output will always be positive pos_output = self.pos_constrain(-self.pos_control.output / self.dis4constV) # -1 = -180/180 < output/180 < 180/180 = 1 ang_output = self.ang_constrain(self.ang_control.output * 2 / 180.) if abs(self.ang_control.output) > self.turn_threshold: if pos_output > 0.1: pos_output = 0.1 return pos_output, ang_output def station_keeping(self, goal_distance, goal_angle): self.pos_station_control.update(goal_distance) self.ang_station_control.update(goal_angle) # pos_output will always be positive pos_output = self.pos_station_constrain( -self.pos_station_control.output / self.dis4constV) # -1 = -180/180 < output/180 < 180/180 = 1 ang_output = self.ang_station_control.output / 180. # if the goal is behind the robot if abs(goal_angle) > 90: pos_output = -pos_output ang_output = -ang_output return pos_output, ang_output def station_keeping_cb(self, req): if req.data == True: self.goal = self.stop_pos self.is_station_keeping = True else: self.goal = self.final_goal self.is_station_keeping = False res = SetBoolResponse() res.success = True res.message = "recieved" return res def cmd_constarin(self, input): if input > self.cmd_ctrl_max: return self.cmd_ctrl_max if input < self.cmd_ctrl_min: return self.cmd_ctrl_min return input def pos_constrain(self, input): if input > self.pos_ctrl_max: return self.pos_ctrl_max if input < self.pos_ctrl_min: return self.pos_ctrl_min return input def ang_constrain(self, input): if input > self.ang_ctrl_max: return self.ang_ctrl_max if input < self.ang_ctrl_min: return self.ang_ctrl_min return input def pos_station_constrain(self, input): if input > self.pos_station_max: return self.pos_station_max if input < self.pos_station_min: return self.pos_station_min return input def initialize_PID(self): self.pos_control.setSampleTime(1) self.ang_control.setSampleTime(1) self.pos_station_control.setSampleTime(1) self.ang_station_control.setSampleTime(1) self.pos_control.SetPoint = 0.0 self.ang_control.SetPoint = 0.0 self.pos_station_control.SetPoint = 0.0 self.ang_station_control.SetPoint = 0.0 def get_goal_angle(self, robot_yaw, robot, goal): robot_angle = np.degrees(robot_yaw) p1 = [robot[0], robot[1]] p2 = [robot[0], robot[1] + 1.] p3 = goal angle = self.get_angle(p1, p2, p3) result = angle - robot_angle result = self.angle_range(-(result + 90.)) return result def get_angle(self, p1, p2, p3): v0 = np.array(p2) - np.array(p1) v1 = np.array(p3) - np.array(p1) angle = np.math.atan2(np.linalg.det([v0, v1]), np.dot(v0, v1)) return np.degrees(angle) def angle_range(self, angle): # limit the angle to the range of [-180, 180] if angle > 180: angle = angle - 360 angle = self.angle_range(angle) elif angle < -180: angle = angle + 360 angle = self.angle_range(angle) return angle def get_distance(self, p1, p2): return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2) def publish_goal(self, goal): marker = Marker() marker.header.frame_id = self.frame_id marker.header.stamp = rospy.Time.now() marker.ns = "pure_pursuit" marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.orientation.w = 1 marker.pose.position.x = goal[0] marker.pose.position.y = goal[1] marker.id = 0 marker.scale.x = 0.6 marker.scale.y = 0.6 marker.scale.z = 0.6 marker.color.a = 1.0 marker.color.g = 1.0 self.pub_goal.publish(marker) def pos_pid_cb(self, config, level): print("Position: [Kp]: {Kp} [Ki]: {Ki} [Kd]: {Kd}\n".format( **config)) Kp = float("{Kp}".format(**config)) Ki = float("{Ki}".format(**config)) Kd = float("{Kd}".format(**config)) self.pos_control.setKp(Kp) self.pos_control.setKi(Ki) self.pos_control.setKd(Kd) return config def ang_pid_cb(self, config, level): print( "Angular: [Kp]: {Kp} [Ki]: {Ki} [Kd]: {Kd}\n".format(**config)) Kp = float("{Kp}".format(**config)) Ki = float("{Ki}".format(**config)) Kd = float("{Kd}".format(**config)) self.ang_control.setKp(Kp) self.ang_control.setKi(Ki) self.ang_control.setKd(Kd) return config def pos_station_pid_cb(self, config, level): print("Position: [Kp]: {Kp} [Ki]: {Ki} [Kd]: {Kd}\n".format( **config)) Kp = float("{Kp}".format(**config)) Ki = float("{Ki}".format(**config)) Kd = float("{Kd}".format(**config)) self.pos_station_control.setKp(Kp) self.pos_station_control.setKi(Ki) self.pos_station_control.setKd(Kd) return config def ang_station_pid_cb(self, config, level): print( "Angular: [Kp]: {Kp} [Ki]: {Ki} [Kd]: {Kd}\n".format(**config)) Kp = float("{Kp}".format(**config)) Ki = float("{Ki}".format(**config)) Kd = float("{Kd}".format(**config)) self.ang_station_control.setKp(Kp) self.ang_station_control.setKi(Ki) self.ang_station_control.setKd(Kd) return config def publish_lookahead(self, robot, lookahead): marker = Marker() marker.header.frame_id = "/map" marker.header.stamp = rospy.Time.now() marker.ns = "pure_pursuit" marker.type = marker.LINE_STRIP marker.action = marker.ADD wp = Point() wp.x, wp.y = robot[:2] wp.z = 0 marker.points.append(wp) wp = Point() wp.x, wp.y = lookahead[0], lookahead[1] wp.z = 0 marker.points.append(wp) marker.id = 0 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 1.0 marker.color.b = 1.0 marker.color.g = 1.0 self.pub_lookahead.publish(marker)
def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) ## parameters self.map_frame = rospy.get_param("~map_frame", 'odom') self.bot_frame = 'base_link' self.switch_thred = 1.5 # change to next lane if reach next self.pursuit = PurePursuit() self.lka = rospy.get_param("~lookahead", 0.5) self.pursuit.set_look_ahead_distance(self.lka) ## node path while not rospy.has_param("/guid_path") and not rospy.is_shutdown(): rospy.loginfo("Wait for /guid_path") rospy.sleep(0.5) self.guid_path = rospy.get_param("/guid_path") self.tag_ang_init = rospy.get_param("/guid_path_ang_init") self.last_node = -1 self.next_node_id = None self.all_anchor_ids = rospy.get_param("/all_anchor_ids") self.joy_lock = False self.get_goal = True self.joint_ang = None ## set first tracking lane self.pub_robot_speech = rospy.Publisher("speech_case", String, queue_size=1) self.pub_robot_go = rospy.Publisher("robot_go", Bool, queue_size=1) rospy.sleep(1) # wait for the publisher to be set well self.set_lane(True) # variable self.target_global = None self.listener = tf.TransformListener() # markers self.pub_goal_marker = rospy.Publisher("goal_marker", Marker, queue_size=1) self.pub_target_marker = rospy.Publisher("target_marker", Marker, queue_size=1) # publisher, subscriber self.pub_pid_goal = rospy.Publisher("pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_box = rospy.Subscriber("anchor_position", PoseDirectional, self.cb_goal, queue_size=1) sub_joy = rospy.Subscriber('joy_teleop/joy', Joy, self.cb_joy, queue_size=1) sub_fr = rospy.Subscriber('front_right/ranges', DeviceRangeArray, self.cb_range, queue_size=1) sub_joint = rospy.Subscriber('/dynamixel_workbench/joint_states', JointState, self.cb_joint, queue_size=1) #Don't update goal too often self.last_update_goal = None self.goal_update_thred = 0.001 self.hist_goal = np.array([]) self.normal = True self.get_goal = True self.timer = rospy.Timer(rospy.Duration(0.1), self.tracking) # print("init done") # prevent sudden stop self.last_plan = None # keep searching until find path or reach search end self.search_angle = 10 * math.pi / 180 self.search_max = 5 ### stupid range drawing # for using tf to draw range br1 = tf2_ros.StaticTransformBroadcaster() br2 = tf2_ros.StaticTransformBroadcaster() # stf.header.frame_id = "uwb_back_left" # stf.child_frame_id = "base_link" # stf.transform.translation.x = -1*r_tag_points[0, 0] # stf.transform.translation.y = -1*r_tag_points[0, 1] # br1.sendTransform(stf) # stf2 = stf # stf2.header.stamp = rospy.Time.now() # stf2.header.frame_id = "uwb_front_right" # stf2.transform.translation.x = -1*r_tag_points[1, 0] # stf2.transform.translation.y = -1*r_tag_points[1, 1] # br2.sendTransform(stf2) stf = TransformStamped() stf.header.stamp = rospy.Time.now() stf.transform.rotation.w = 1 stf.header.frame_id = "base_link" stf.child_frame_id = "uwb_back_left" stf.transform.translation.x = r_tag_points[0, 0] stf.transform.translation.y = r_tag_points[0, 1] stf2 = TransformStamped() stf2.header.stamp = rospy.Time.now() stf2.transform.rotation.w = 1 stf2.header.frame_id = "base_link" stf2.child_frame_id = "uwb_front_right" stf2.transform.translation.x = r_tag_points[1, 0] stf2.transform.translation.y = r_tag_points[1, 1]
class Navigation(object): def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) ## parameters self.map_frame = rospy.get_param("~map_frame", 'odom') self.bot_frame = None self.switch_thred = 1 # change to next lane if reach next self.pursuit = PurePursuit() self.pursuit.set_look_ahead_distance(0.1) ## node path while not rospy.has_param("/guid_path"): rospy.loginfo("Wait for /guid_path") rospy.sleep(0.5) self.guid_path = rospy.get_param("/guid_path") self.last_node = -1 self.all_anchor_ids = rospy.get_param("/all_anchor_ids") self.joy_lock = False ## set first tracking lane self.set_lane(True) # variable self.target_global = None self.listener = tf.TransformListener() # markers self.pub_goal_marker = rospy.Publisher("goal_marker", Marker, queue_size=1) self.pub_target_marker = rospy.Publisher("target_marker", Marker, queue_size=1) # publisher, subscriber self.pub_pid_goal = rospy.Publisher("pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_box = rospy.Subscriber("anchor_pose", PoseDirectional, self.cb_goal, queue_size=1) sub_joy = rospy.Subscriber('joy_teleop/joy', Joy, self.cb_joy, queue_size=1) self.timer = rospy.Timer(rospy.Duration(0.1), self.tracking) # print("init done") def tracking(self, event): # print("tracking loop") if self.target_global is None: # rospy.logerr("%s : no goal" % rospy.get_name()) return else: rospy.loginfo("%s :have seen goal" % rospy.get_name()) end_p = self.transform_pose(self.target_global.pose, self.bot_frame, self.map_frame) self.pub_target_marker.publish(self.to_marker(end_p, [0, 0, 1])) d = math.sqrt(end_p.pose.position.x**2 + end_p.pose.position.y**2) if d <= self.switch_thred: rospy.logwarn("goal reached, to next goal") self.target_global = None self.set_lane(True) return start_p = PoseStamped() start_p.pose.position.x = 1 start_p.pose.position.y = 0 start_p.pose.position.z = 0 req_path = GetPlanRequest() req_path.start = start_p req_path.goal = end_p try: res_path = self.req_path_srv(req_path) except: rospy.logerr("%s : path request fail" % rospy.get_name()) return self.pursuit.set_path(res_path.plan) goal = self.pursuit.get_goal(start_p) if goal is None: rospy.logwarn("goal reached, to next goal") self.target_global = None self.set_lane(True) return goal = self.transform_pose(goal.pose, self.map_frame, self.bot_frame) goal.header.frame_id = self.map_frame self.pub_goal_marker.publish(self.to_marker(goal)) self.pub_pid_goal.publish(goal) def cb_goal(self, msg): self.target_global = msg.pose # posestamped self.bot_frame = self.target_global.header.frame_id self.target_global.pose = self.transform_pose(msg.pose.pose, self.map_frame, msg.header.frame) self.target_global.header.frame_id = self.map_frame def cb_joy(self, msg): switch = msg.axes[-1] if switch == 0: self.joy_lock = False if self.joy_lock: return switch = msg.axes[-1] if switch == 1: rospy.loginfo("Joy to the next lane.") self.set_lane(True) self.joy_lock = True elif switch == -1: rospy.loginfo("Joy to the last lane.") self.set_lane(False) self.joy_lock = True def set_lane(self, next): # to next node if next: self.last_node += 1 else: self.last_node -= 1 if self.last_node >= len(self.guid_path) - 1: rospy.loginfo("It's the last lane.") self.last_node -= 1 return elif self.last_node < 0: rospy.loginfo("Back to first lane.") self.last_node = 0 # set last, next node (anchor) rospy.set_param("/guid_lane_last", self.all_anchor_ids[self.guid_path[self.last_node]]) rospy.set_param( "/guid_lane_next", self.all_anchor_ids[self.guid_path[self.last_node + 1]]) # set pozyx ranging tag id try: rospy.delete_param("/anchorballs") except KeyError: rospy.logerr("No such param /anchorballs") pozyx_id = {} pozyx_id[self.guid_path[self.last_node]] = self.all_anchor_ids[ self.guid_path[self.last_node]] pozyx_id[self.guid_path[self.last_node + 1]] = self.all_anchor_ids[ self.guid_path[self.last_node + 1]] rospy.set_param("/anchorballs", pozyx_id) def to_marker(self, goal, color=[0, 1, 0]): marker = Marker() marker.header.frame_id = goal.header.frame_id marker.header.stamp = rospy.Time.now() marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.orientation.w = 1 marker.pose.position.x = goal.pose.position.x marker.pose.position.y = goal.pose.position.y marker.id = 0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] return marker def transform_pose(self, pose, target_frame, source_frame): try: (trans_c, rot_c) = self.listener.lookupTransform(target_frame, source_frame, rospy.Time(0)) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("faile to catch tf %s 2 %s" % (target_frame, source_frame)) return trans_mat = tf.transformations.translation_matrix(trans_c) rot_mat = tf.transformations.quaternion_matrix(rot_c) tran_mat = np.dot(trans_mat, rot_mat) # print(trans_c) target_mat = np.array([[1, 0, 0, pose.position.x], [0, 1, 0, pose.position.y], [0, 0, 1, pose.position.z], [0, 0, 0, 1]]) target = np.dot(tran_mat, target_mat) quat = tf.transformations.quaternion_from_matrix(target) trans = tf.transformations.translation_from_matrix(target) t_pose = PoseStamped() t_pose.header.frame_id = target_frame t_pose.pose.position.x = trans[0] t_pose.pose.position.y = trans[1] t_pose.pose.position.z = trans[2] t_pose.pose.orientation.x = quat[0] t_pose.pose.orientation.y = quat[1] t_pose.pose.orientation.z = quat[2] t_pose.pose.orientation.w = quat[3] return t_pose
def test_find_closest_point( self): # only functions with 'test_'-prefix will be run! # Set up path pp = PurePursuit() pp.path = Path() pose0 = PoseStamped() pose0.pose.position.x = 0.0 pose0.pose.position.y = 0.0 pp.path.poses.append(pose0) pose1 = PoseStamped() pose1.pose.position.x = 1.0 pose1.pose.position.y = 0.0 pp.path.poses.append(pose1) pose2 = PoseStamped() pose2.pose.position.x = 2.0 pose2.pose.position.y = 1.0 pp.path.poses.append(pose2) # Set up test inputs x = [ np.array([-1., 0.]), np.array([0., 0.1]), np.array([0.5, 0.5]), np.array([0.9, -1.]), np.array([1.5, 0.5]), np.array([2., 0.5]), np.array([3., 3.]) ] #x = np.array([-1., 0., 0.5, 0.9, 1.5, 2.0, 3.0]) #y = np.array([ 0., 0.1, 0.5, -1., 0.5, 0.5, 3.0]) # Desired outputs pts_true = [ np.array([0., 0.]), np.array([0., 0.]), np.array([0.5, 0.]), np.array([0.9, 0.]), np.array([1.5, 0.5]), np.array([1.75, 0.75]), np.array([2., 1.]) ] dists_true = [1., 0.1, 0.5, 1.0, 0., 0.25 * np.sqrt(2.), np.sqrt(5.)] segs_true = [0, 0, 0, 0, 1, 1, 1] # Ensure that calculated outputs match desired outputs err_msg_pt = "test point ({},{}) has the wrong closest point ({}, {}) instead of ({}, {})" err_msg_dist = "test point ({},{}) has the wrong distance ({} instead of {})" err_msg_seg = "test point ({},{}) has the wrong segment ({} instead of {})" for i in range(0, len(x)): (pt, dist, seg) = pp.find_closest_point(x[i]) self.assertTrue( np.linalg.norm(pt - pts_true[i]) < 1e-6, err_msg_pt.format(x[i][0], x[i][1], pt[0], pt[1], pts_true[i][0], pts_true[i][1])) self.assertTrue( np.abs(dist - dists_true[i]) < 1e-6, err_msg_dist.format(x[i][0], x[i][1], dist, dists_true[i])) self.assertEqual( seg, segs_true[i], err_msg_seg.format(x[i][0], x[i][1], seg, segs_true[i]))