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