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