class Robot_PID():
def __init__(self):
self.node_name = rospy.get_name()
self.dis4constV = 3. # Distance for constant velocity
self.pos_ctrl_max = 0.6
self.pos_ctrl_min = 0
self.ang_ctrl_max = 0.4
self.ang_ctrl_min = -0.4
self.turn_threshold = 30
self.cmd_ctrl_max = 0.1
self.cmd_ctrl_min = -0.1
self.arrived_dis = 0.5 # meters
self.frame_id = 'map'
self.emergency_stop = False
self.final_goal = None # The final goal that you want to arrive
self.goal = self.final_goal
self.cmd = Twist()
self.spaceship_cmd = MotorsCmd()
rospy.loginfo("[%s] Initializing " %(self.node_name))
self.sub_goal = rospy.Subscriber("pursue_point", PoseStamped, self.goal_cb, queue_size=1)
self.pub_spaceship_cmd = rospy.Publisher("/spaceship/motors_cmd", MotorsCmd, queue_size = 1)
self.pub_cmd = rospy.Publisher("/husky_velocity_controller/cmd_vel", Twist, queue_size = 1)
self.pub_cmd2d = rospy.Publisher("/spaceship/vel_cmd", Twist2D, queue_size = 1)
self.cmd2d = Twist2D()
self.pub_goal = rospy.Publisher("/goal_point", Marker, queue_size = 1)
self.emergency_stop_srv = rospy.Service("/emergency_stop", SetBool, self.emergency_stop_cb)
self.pos_control = PID_control("Position")
self.ang_control = PID_control("Angular")
self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")
self.initialize_PID()
while not rospy.is_shutdown():
#self.pub_spaceship_cmd.publish(self.spaceship_cmd)
#self.pub_cmd.publish(self.cmd)
##### omni version #####
self.pub_cmd2d.publish(self.cmd2d)
if self.goal is not None:
self.publish_goal(self.goal)
rospy.sleep(0.1)
def goal_cb(self, p):
robot_position = [0, 0]
self.final_goal = [p.pose.position.x, p.pose.position.y]
if self.final_goal == [0, 0]:
cmd_msg = Twist()
cmd_msg.linear.x = 0
cmd_msg.angular.z = 0
self.cmd = cmd_msg
return
self.goal = self.final_goal
goal_distance = self.get_distance(robot_position, self.goal)
goal_angle = self.get_goal_angle(0, robot_position, self.goal)
if goal_distance < self.arrived_dis or self.emergency_stop:
rospy.loginfo("Stop!!!")
pos_output, ang_output = (0, 0)
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.cmd = cmd_msg
##### omni version #####
cmd2d = Twist2D()
cmd2d.v = pos_output + 0.2
cmd2d.omega = ang_output
self.cmd2d = cmd2d
print('===',self.cmd2d)
#spaceship_cmd_msg = MotorsCmd()
#self.spaceship_cmd.vel_front_left = 1300 + (pos_output + ang_output) * 600 / (self.pos_ctrl_max + self.ang_ctrl_max)
#self.spaceship_cmd.vel_front_right = 1300 + (pos_output - ang_output) * 600 / ((self.pos_ctrl_max + self.ang_ctrl_max))
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*3/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 emergency_stop_cb(self, req):
if req.data == True:
self.emergency_stop = True
else:
self.emergency_stop = 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 initialize_PID(self):
self.pos_control.setSampleTime(1)
self.ang_control.setSampleTime(1)
self.pos_control.SetPoint = 0.0
self.ang_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
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.turn_threshold = 20
self.slow_speed = 0.1
self.cmd_ctrl_max = 0.7
self.cmd_ctrl_min = -0.7
self.arrived_dis = 0.5 # meters
self.frame_id = 'map'
self.emergency_stop = False
self.final_goal = None # The final goal that you want to arrive
self.goal = self.final_goal
self.arrive = True
rospy.loginfo("[%s] Initializing " % (self.node_name))
self.sub_goal = rospy.Subscriber("pursue_point",
PoseStamped,
self.goal_cb,
queue_size=1)
self.sub_arrive = rospy.Subscriber("arrive",
Bool,
self.arrive_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_goal = rospy.Publisher("goal_point", Marker, queue_size=1)
self.emergency_stop_srv = rospy.Service("/emergency_stop", SetBool,
self.emergency_stop_cb)
self.pos_control = PID_control("Position")
self.ang_control = PID_control("Angular")
self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")
self.initialize_PID()
def arrive_cb(self, msg):
self.arrive = msg.data
def odom_cb(self, msg):
self.frame_id = msg.header.frame_id
robot_position = [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
#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.arrived_dis or self.emergency_stop or self.arrive:
rospy.loginfo("Stop!!!")
pos_output, ang_output = (0, 0)
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 * 3 / 180.)
if abs(self.ang_control.output) > self.turn_threshold:
if pos_output > 0.1:
pos_output = self.slow_speed
return pos_output, ang_output
def goal_cb(self, p):
self.final_goal = [p.pose.position.x, p.pose.position.y]
self.goal = self.final_goal
def emergency_stop_cb(self, req):
if req.data == True:
self.emergency_stop = True
else:
self.emergency_stop = 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 initialize_PID(self):
self.pos_control.setSampleTime(1)
self.ang_control.setSampleTime(1)
self.pos_control.SetPoint = 0.0
self.ang_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
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.turn_threshold = 20
self.slow_speed = 0.1
self.cmd_ctrl_max = 0.7
self.cmd_ctrl_min = -0.7
self.arrived_dis = 0.2 # meters
#self.frame_id = 'map'
self.frame_id = 'laser_frame'
#self.frame_id = 'camera_link'
self.emergency_stop = True
self.final_goal = None # The final goal that you want to arrive
self.goal = self.final_goal
rospy.loginfo("[%s] Initializing " % (self.node_name))
self.use_odom = rospy.get_param("use_odom")
if not self.use_odom:
rospy.Timer(rospy.Duration(1 / 30.), self.timer_cb)
self.cmd = Twist()
self.sub_goal = rospy.Subscriber("pursue_point",
PoseStamped,
self.goal_cb,
queue_size=1)
self.pub_arrive = rospy.Publisher("arrive", Bool, queue_size=1)
if self.use_odom:
rospy.Subscriber('odometry/ground_truth',
Odometry,
self.odom_cb,
queue_size=1,
buff_size=2**24)
self.pub_cmd = rospy.Publisher("cmd_vel", Twist, queue_size=1)
self.pub_goal = rospy.Publisher("goal_point", Marker, queue_size=1)
self.pub_estop = rospy.Publisher("/racecar/stop_mode",
Bool,
queue_size=1)
self.emergency_stop_srv = rospy.Service("emergency_stop", SetBool,
self.emergency_stop_cb)
self.pos_control = PID_control("Position")
self.ang_control = PID_control("Angular")
self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")
self.initialize_PID()
# sub joy to switch on or off
self.auto = False
self.sub_joy = rospy.Subscriber('joy', Joy, self.cb_joy, queue_size=1)
def odom_cb(self, msg):
self.frame_id = msg.header.frame_id
robot_position = [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
#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.arrived_dis or self.emergency_stop:
print "Goal: ", self.goal, ", robot at: ", robot_position
print goal_distance
rospy.loginfo("Stop in odom_cb!!!")
self.pub_arrive.publish(Bool(True))
print goal_distance, " ", self.emergency_stop, " "
pos_output, ang_output = (0, 0)
self.goal = None
else:
pos_output, ang_output = self.control(goal_distance, goal_angle)
cmd_msg = Twist()
cmd_msg.linear.x = pos_output * 2
cmd_msg.angular.z = ang_output
if not self.emergency_stop:
self.pub_cmd.publish(cmd_msg)
if self.goal is not None:
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 * 3 / 180.)
if abs(self.ang_control.output) > self.turn_threshold:
if pos_output > 0.1:
pos_output = self.slow_speed
return pos_output, ang_output
def goal_cb(self, p):
print('goal_cb')
self.final_goal = [p.pose.position.x, p.pose.position.y]
self.goal = self.final_goal
# New goal, publish arrive to false
self.pub_arrive.publish(Bool(False))
if not self.use_odom:
robot_position = [0.0, 0.0]
yaw = 0
goal_distance = self.get_distance(robot_position, self.goal)
goal_angle = self.get_goal_angle(yaw, robot_position, self.goal)
estop = Bool()
if goal_distance < self.arrived_dis:
rospy.loginfo("Arrived!!!")
self.pub_arrive.publish(Bool(True))
print goal_distance
pos_output, ang_output = (0, 0)
estop.data = True
self.pub_estop.publish(estop)
self.goal = None
elif self.emergency_stop:
rospy.loginfo("JOYSTICK: Stop!!!")
self.pub_arrive.publish(Bool(True))
pos_output, ang_output = (0, 0)
estop.data = True
self.pub_estop.publish(estop)
self.publish_goal(self.goal)
return
else:
estop.data = False
self.pub_estop.publish(estop)
pos_output, ang_output = self.control(goal_distance,
goal_angle)
self.cmd = Twist()
self.cmd.linear.x = pos_output * 2
self.cmd.angular.z = ang_output
if self.auto:
self.pub_cmd.publish(self.cmd)
print('Pub cmd')
if self.goal is not None:
self.publish_goal(self.goal)
def emergency_stop_cb(self, req):
if req.data == True:
self.emergency_stop = True
rospy.loginfo("Mode: Joystick")
else:
self.emergency_stop = False
rospy.loginfo("Mode: Autonomous")
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 initialize_PID(self):
self.pos_control.setSampleTime(1)
self.ang_control.setSampleTime(1)
self.pos_control.SetPoint = 0.0
self.ang_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()
if self.use_odom: marker.header.frame_id = self.frame_id
else: marker.header.frame_id = "laser_frame"
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 timer_cb(self, event):
if not self.emergency_stop:
self.pub_cmd.publish(self.cmd)
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 cb_joy(self, joy_msg):
# MODE D
start_button = 1
back_button = 3
# Start button
if joy_msg.buttons[start_button] == 1:
self.auto = True
rospy.loginfo('go auto')
elif joy_msg.buttons[back_button] == 1:
self.auto = False
rospy.loginfo('go manual')
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 Tracking():
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " % (self.node_name))
self.ROBOT_NUM = 3
self.wavm_labels = ["wamv", ""]
#rospy.Subscriber('/pcl_points_img', PoseArray, self.call_back, queue_size = 1, buff_size = 2**24)
self.net = build_ssd('test', 300, 3) # initialize SSD
self.net.load_weights(
'/home/arg_ws3/argbot/catkin_ws/src/tracking/src/ssd300_wamv_5000.pth'
)
if torch.cuda.is_available():
self.net = self.net.cuda()
# Image definition
self.width = 1280
self.height = 960
self.h_w = 10.
self.const_SA = 0.17
self.bridge = CvBridge()
self.predict_prob = 0.5
self.pos_ctrl_max = 1
self.pos_ctrl_min = -1
self.cmd_ctrl_max = 0.95
self.cmd_ctrl_min = -0.95
self.station_keeping_dis = 3.5 # meters
self.frame_id = 'odom'
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
rospy.loginfo("[%s] Initializing " % (self.node_name))
#self.image_sub = rospy.Subscriber("/BRIAN/camera_node/image/compressed", Image, self.img_cb, queue_size=1)
self.image_sub = rospy.Subscriber("/detecter/predictions",
Boxlist,
self.box_cb,
queue_size=1,
buff_size=2**24)
self.pub_cmd = rospy.Publisher("/MONICA/cmd_drive",
UsvDrive,
queue_size=1)
self.pub_goal = rospy.Publisher("/goal_point", Marker, queue_size=1)
self.image_pub = rospy.Publisher("/predict_img", Image, queue_size=1)
self.station_keeping_srv = rospy.Service("/station_keeping", SetBool,
self.station_keeping_cb)
self.pos_control = PID_control("Position_tracking")
self.ang_control = PID_control("Angular_tracking")
self.ang_station_control = PID_control("Angular_station")
self.pos_station_control = PID_control("Position_station")
self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb,
"Position_tracking")
self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb,
"Angular_tracking")
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 box_cb(self, msg):
self.width = msg.image_width
self.height = msg.image_height
try:
np_arr = np.fromstring(msg.image.data, np.uint8)
cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
#cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
print(e)
#(rows, cols, channels) = cv_image.shape
boxes = msg.list
predict = get_control_info(boxes, cv_image)
if predict is None:
return
angle, dis = predict[0], predict[1]
self.tracking_control(angle, dis)
def tracking_control(self, goal_angle, goal_distance):
if 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 = UsvDrive()
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)
#self.publish_goal(self.goal)
def get_control_info(self, boxes, img):
# Image Preprocessing (vgg use BGR image as training input)
confidence = 0
bbox = None
for box in boxes:
if box.confidence > confidence:
bbox = box
angle, dis, center = self.BBx2AngDis(bbox)
cv2.circle(img, (int(center[0]), int(center[1])), 10, (0, 0, 255), -1)
cv2.rectangle(img, (int(coords[0][0]), int(coords[0][1])),\
(int(coords[0][0] + coords[1]), int(coords[0][1] + coords[2])),(0,0,255),5)
try:
img = self.draw_cmd(img, dis, angle)
self.image_pub.publish(self.bridge.cv2_to_imgmsg(img, "bgr8"))
except CvBridgeError as e:
print(e)
return angle, dis
def draw_cmd(self, img, dis, angle):
v = dis
omega = angle
rad = omega * math.pi / 2. # [-1.57~1.57]
rad = rad - math.pi / 2. # rotae for correct direction
radius = 120
alpha = 0.3
v_length = radius * math.sqrt(v**2 + v**2) / math.sqrt(1**2 + 1**2)
if v_length > radius:
v_length = radius
x = v_length * math.cos(rad)
y = v_length * math.sin(rad)
x_max = radius * math.cos(rad)
y_max = radius * math.sin(rad)
center = (int(self.width / 2.), int(self.height))
draw_img = img.copy()
cv2.circle(draw_img, center, radius, (255, 255, 255), 10)
cv2.addWeighted(draw_img, alpha, img, 1 - alpha, 0, img)
#cv2.circle(img, (int(center[0]+x_max), int(center[1]+y_max)), 15, (255, 255, 255), -1)
cv2.arrowedLine(img, center, (int(center[0] + x), int(center[1] + y)),
(255, 255, 255), 7)
return img
def BBx2AngDis(self, bbox):
x = bbox.x
y = bbox.y
w = bbox.w
h = bbox.h
center = (x + w / 2., y + h / 2.)
angle = (center[0] - self.width / 2.) / (self.width / 2.)
dis = (self.height - h) / (self.height)
return angle, dis, center
def control(self, goal_distance, goal_angle):
self.pos_control.update(10 * (goal_distance - self.const_SA))
self.ang_control.update(goal_angle)
# pos_output will always be positive
pos_output = self.pos_constrain(self.pos_control.output)
# -1 = -180/180 < output/180 < 180/180 = 1
ang_output = self.ang_control.output / (self.width / 2.)
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 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
class Robot_PID():
def __init__(self):
self.node_name = rospy.get_name()
self.dis4constV = 5.0 # Distance for constant velocity
self.max_dis_ratiao = 0.8
self.pos_ctrl_max = 1
self.pos_ctrl_min = -1
self.pos_station_max = 0.8
self.pos_station_min = -0.8
self.cmd_ctrl_max = 1
self.cmd_ctrl_min = -1
self.station_keeping_dis = 3.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.heading = 0
rospy.loginfo("[%s] Initializing " % (self.node_name))
# Param
self.sim = rospy.get_param('sim', False)
self.tune = rospy.get_param('tune', True)
self.sub_goal = rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.goal_cb,
queue_size=1)
rospy.Subscriber('localization_gps_imu/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",
VelocityVector,
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)
if self.tune:
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.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()
else:
print 'no working...'
def odom_cb(self, msg):
self.frame_id = msg.header.frame_id
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
#yaw = yaw + np.pi/2
goal_vector = self.get_distance(robot_position, self.goal)
goal_angle = self.get_goal_angle(yaw, robot_position, self.goal)
goal_distance = math.sqrt(goal_vector[0]**2 + goal_vector[1]**2)
if goal_distance < self.station_keeping_dis or self.is_station_keeping:
rospy.loginfo("Station Keeping")
head_angle = self.angle_range(np.degrees(yaw) - self.heading)
pos_x_output, pos_y_output, ang_output = self.station_keeping(
head_angle, goal_distance, goal_angle)
else:
rospy.loginfo("Navigating")
head_angle = goal_angle
pos_x_output, pos_y_output, ang_output = self.control(
head_angle, goal_distance, goal_angle)
if self.sim:
cmd_msg = UsvDrive()
else:
cmd_msg = VelocityVector()
cmd_msg.x = self.cmd_constarin(pos_x_output)
cmd_msg.y = self.cmd_constarin(pos_y_output)
cmd_msg.angular = self.cmd_constarin(ang_output)
print("heading %f" % self.heading)
print("angular %f" % cmd_msg.angular)
print("compare %f" % np.degrees(yaw))
self.pub_cmd.publish(cmd_msg)
self.publish_goal(self.goal)
def control(self, head_angle, goal_distance, goal_angle):
self.pos_control.update(goal_distance)
dis_ratiao = -self.pos_control.output / self.dis4constV
dis_ratiao = max(min(dis_ratiao, self.max_dis_ratiao), 0)
print("dis ratiao%f" % dis_ratiao)
pos_x_output = math.sin(math.radians(goal_angle))
pos_y_output = math.cos(math.radians(goal_angle))
pos_x_output = self.pos_constrain(pos_x_output * dis_ratiao)
pos_y_output = self.pos_constrain(pos_y_output * dis_ratiao)
# -1 = -180/180 < output/180 < 180/180 = 1
self.ang_control.update(head_angle)
ang_output = self.ang_control.output / 180. * -1
return pos_x_output, pos_y_output, ang_output
def station_keeping(self, head_angle, goal_distance, goal_angle):
self.pos_station_control.update(goal_distance)
dis_ratiao = -self.pos_station_control.output
dis_ratiao = max(min(dis_ratiao, self.max_dis_ratiao), 0)
pos_x_output = math.sin(math.radians(goal_angle))
pos_y_output = math.cos(math.radians(goal_angle))
pos_x_output = self.pos_constrain(pos_x_output * dis_ratiao)
pos_y_output = self.pos_constrain(pos_y_output * dis_ratiao)
# -1 = -180/180 < output/180 < 180/180 = 1
self.ang_station_control.update(head_angle)
ang_output = self.ang_station_control.output / 180. * -1
return pos_x_output, pos_y_output, ang_output
def goal_cb(self, p):
self.final_goal = [p.pose.position.x, p.pose.position.y]
self.goal = self.final_goal
quat = (p.pose.orientation.x,\
p.pose.orientation.y,\
p.pose.orientation.z,\
p.pose.orientation.w)
_, _, yaw = tf.transformations.euler_from_quaternion(quat)
self.heading = np.degrees(yaw)
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 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 [p1[0] - p2[0], p1[1] - p2[1]]
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
class Tracking():
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " % (self.node_name))
# Image definition
self.width = 640
self.height = 480
self.h_w = 10.
self.const_SA = 0.4
self.predict_prob = 0.5
self.pos_ctrl_max = 1
self.pos_ctrl_min = -1
self.cmd_ctrl_max = 1
self.cmd_ctrl_min = -1
rospy.loginfo("[%s] Initializing " % (self.node_name))
self.image_sub = rospy.Subscriber("BoundingBoxes",
BoundingBoxes,
self.box_cb,
queue_size=1,
buff_size=2**24)
self.pub_cmd = rospy.Publisher('track_vel', Twist, queue_size=1)
self.cmd_msg = Twist()
self.pos_control = PID_control("Position_tracking")
self.ang_control = PID_control("Angular_tracking")
self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb,
"Position_tracking")
self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb,
"Angular_tracking")
self.initialize_PID()
self.timer = rospy.Timer(rospy.Duration(0.2), self.cb_publish)
def cb_publish(self, event):
self.pub_cmd.publish(self.cmd_msg)
def box_cb(self, msg):
goal_angle, goal_distance = self.BBx2AngDis(msg.bounding_boxes[0])
pos_output, ang_output = self.control(goal_distance, goal_angle)
self.cmd_msg = Twist()
self.cmd_msg.linear.x = pos_output
self.cmd_msg.angular.z = ang_output
def BBx2AngDis(self, bbox):
center = [(bbox.xmin + bbox.xmax) / 2., (bbox.ymin + bbox.ymax) / 2.]
angle = (center[0] - self.width / 2.) / (self.width / 2.)
dis = float(self.height - (bbox.ymax - bbox.ymin)) / (self.height)
return angle, dis
def control(self, goal_distance, goal_angle):
self.pos_control.update(5 * (goal_distance - self.const_SA))
self.ang_control.update(goal_angle)
# pos_output will always be positive
pos_output = -self.pos_constrain(self.pos_control.output)
# -1 = -180/180 < output/180 < 180/180 = 1
ang_output = self.ang_control.output
return pos_output, ang_output
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 initialize_PID(self):
self.pos_control.setSampleTime(1)
self.ang_control.setSampleTime(1)
self.pos_control.SetPoint = 0.0
self.ang_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)
# 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 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 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
class BoatHRVO(object):
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing" % self.node_name)
# initiallize PID
self.dis_pid = PID_control("distance_control")
self.angu_pid = PID_control("angular_control")
self.dis_server = Server(dis_PIDConfig, self.cb_dis_pid,
"distance_control")
self.ang_server = Server(ang_PIDConfig, self.cb_ang_pid,
"angular_control")
self.dis_pid.setSampleTime(0.1)
self.angu_pid.setSampleTime(0.1)
self.dis_pid.SetPoint = 0
self.angu_pid.SetPoint = 0
# setup publisher
# self.pub_v1 = rospy.Publisher("cmd_vel", Twist, queue_size=1)
# self.sub_p3d1 = rospy.Subscriber(
# "p3d_odom", Odometry, self.cb_boat1_odom, queue_size=1)
# initiallize boat status
self.boat_odom = Odometry()
self.yaw = 0
# initiallize HRVO environment
self.ws_model = dict()
# robot radius
self.ws_model['robot_radius'] = 1
print os.path.dirname(__file__)
data = pd.read_csv(
"/home/developer/vrx/catkin_ws/src/vrx/vrx_gazebo/worlds/block_position.csv"
)
objs = []
for idx, item in data.iterrows():
objs.append([item['x'], item['y'], objs_dict[item['object']]])
self.ws_model['circular_obstacles'] = objs
# rectangular boundary, format [x,y,width/2,heigth/2]
self.ws_model['boundary'] = []
self.position = [[0, 0]]
self.goal = self.random_goal()
# print(self.position)
# print(self.goal)
self.velocity = [[0, 0]]
self.v_max = [1]
# timer
# self.timer = rospy.Timer(rospy.Duration(0.2), self.cb_hrvo)
def random_goal(self):
# random angle
alpha = 2 * math.pi * random.random()
# random radius
r = 50 * math.sqrt(2)
# calculating coordinates
x = r * math.cos(alpha)
y = r * math.sin(alpha)
return [[x, y]]
def start_hrvo(self):
env = gym_env.SubtCaveNobackEnv()
epoch = 0
iteration = 60
record = np.zeros([iteration, 2])
for i in range(iteration):
start_time = time.time()
ep_reward = 0
step = 0
distance = 0
while True:
v_des = compute_V_des(self.position, self.goal, self.v_max)
self.velocity = RVO_update(self.position, v_des, self.velocity,
self.ws_model)
dis, angle = self.process_ang_dis(self.velocity[0][0],
self.velocity[0][1],
self.yaw)
self.position, self.yaw, out_bound, r, done, info = env.step(
[dis * 0.4, angle * 0.4])
if out_bound or done:
self.goal = self.random_goal()
if done or env.total_dis > 600:
record[epoch][0] = env.total_dis
record[epoch][1] = time.time() - start_time
s = env.reset()
break
print epoch, record[epoch]
epoch += 1
folder = os.getcwd()
record_name = 'hrvo.pkl'
fileObject = open(folder + "/" + record_name, 'wb')
pkl.dump(record, fileObject)
fileObject.close()
def process_ang_dis(self, vx, vy, yaw):
dest_yaw = math.atan2(vy, vx)
angle = dest_yaw - yaw
if angle > np.pi:
angle = angle - 2 * np.pi
if angle < -np.pi:
angle = angle + 2 * np.pi
angle = angle / np.pi
dis = math.sqrt(vx**2 + vy**2)
# print "pos %2.2f, %2.2f" % (self.position[0][0], self.position[0][1])
# print "goal %2.2f, %2.2f" % (self.goal[0][0], self.goal[0][1])
# print "dest_yaw %2.2f" % dest_yaw
# print "yaw %2.2f" % yaw
# print "angle %2.2f" % angle
# print "dis %2.2f\n" % dis
dis = max(min(dis, 1), -1)
angle = max(min(angle, 1), -1)
return dis, angle
def update_all(self):
self.position = []
# update position
pos = [
self.boat_odom.pose.pose.position.x,
self.boat_odom.pose.pose.position.y
]
self.position.append(pos)
# update orientation
quaternion = (self.boat_odom.pose.pose.orientation.x,
self.boat_odom.pose.pose.orientation.y,
self.boat_odom.pose.pose.orientation.z,
self.boat_odom.pose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
self.yaw = euler[2]
def cb_boat1_odom(self, msg):
self.boat_odom = msg
def cb_dis_pid(self, config, level):
print("distance: [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.dis_pid.setKp(Kp)
self.dis_pid.setKi(Ki)
self.dis_pid.setKd(Kd)
return config
def cb_ang_pid(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.angu_pid.setKp(Kp)
self.angu_pid.setKi(Ki)
self.angu_pid.setKd(Kd)
return config
class Robot_PID():
def __init__(self):
cmd_ratio = rospy.get_param("~cmd_ratio", 1)
self.dis4constV = 1. # Distance for constant velocity
self.pos_ctrl_max = 1
self.pos_ctrl_min = 0.0
self.cmd_ctrl_max = 1 * cmd_ratio
self.cmd_ctrl_min = -1 * cmd_ratio
self.goal = None
self.robot_pos = None
self.robot_yaw = None
rospy.loginfo("[%s] Initializing " % rospy.get_name())
self.pub_cmd = rospy.Publisher("cmd_vel", Twist, queue_size=1)
self.pos_control = PID_control("Position")
self.ang_control = PID_control("Angular")
self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")
self.initialize_PID()
self.sub_goal = rospy.Subscriber("pid_control/goal",
PoseStamped,
self.cb_goal,
queue_size=1)
# sub joy to switch on or off
self.auto = False
self.sub_joy = rospy.Subscriber('joy_teleop/joy',
Joy,
self.cb_joy,
queue_size=1)
# tf v.s. Odometry => depend on user and scenario
self.use_odom = rospy.get_param("~use_odom", True)
self.use_tf = rospy.get_param("~use_tf", False)
if self.use_odom:
self.sub_pose = rospy.Subscriber('pid_control/pose',
Odometry,
self.cb_pose,
queue_size=1)
if self.use_tf:
self.map_frame = rospy.get_param("~map_frame", 'map')
self.robot_frame = rospy.get_param("~robot_frame", 'base_link')
self.listener = tf.TransformListener()
def cb_pose(self, msg):
self.robot_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_yaw = yaw
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 cb_goal(self, p):
rospy.logwarn("PID_control_cb_goal")
self.goal = [p.pose.position.x, p.pose.position.y]
if self.use_tf:
try:
(trans_c, rot_c) = self.listener.lookupTransform(
self.map_frame, self.robot_frame, rospy.Time(0))
self.robot_pos = [trans_c[0], trans_c[1]]
_, _, self.robot_yaw = tf.transformations.euler_from_quaternion(
rot_c)
except (tf.Exception, tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logerr("faile to catch tf %s 2 %s" %
(target_frame, source_frame))
return
if self.robot_pos is None:
rospy.logwarn("%s : no robot pose" % rospy.get_name())
return
# self.robot_yaw = self.robot_yaw + np.pi/2
goal_distance = self.get_distance(self.robot_pos, self.goal)
goal_angle = self.get_goal_angle(self.robot_yaw, self.robot_pos,
self.goal)
pos_output, ang_output = self.control(goal_distance, goal_angle)
if self.auto:
cmd_msg = Twist()
cmd_msg.linear.x = self.cmd_constarin(pos_output)
cmd_msg.angular.z = self.cmd_constarin(ang_output)
self.pub_cmd.publish(cmd_msg)
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 initialize_PID(self):
self.pos_control.setSampleTime(1)
self.ang_control.setSampleTime(1)
self.pos_control.SetPoint = 0.0
self.ang_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)
# 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 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 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 cb_joy(self, joy_msg):
# MODE D
start_button = 7
back_button = 6
# Start button
if (joy_msg.buttons[start_button] == 1) and not self.auto:
self.auto = True
rospy.loginfo('go auto')
elif joy_msg.buttons[back_button] == 1 and self.auto:
self.auto = False
rospy.loginfo('go manual')
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 Robot_PID():
def __init__(self):
self.dis4constV = 1. # Distance for constant velocity
self.pos_ctrl_max = 1
self.pos_ctrl_min = 0.0
self.cmd_ctrl_max = 1
self.cmd_ctrl_min = -1
self.goal = None
self.robot_pos = None
self.robot_yaw = None
rospy.loginfo("[%s] Initializing " % rospy.get_name())
self.pub_cmd = rospy.Publisher("cmd_vel", Twist, queue_size=1)
self.pos_control = PID_control("Position")
self.ang_control = PID_control("Angular")
self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")
self.initialize_PID()
self.sub_goal = rospy.Subscriber("pid_control/goal",
PoseStamped,
self.cb_goal,
queue_size=1)
self.sub_pose = rospy.Subscriber('pose',
PoseStamped,
self.cb_pose,
queue_size=1)
def cb_pose(self, msg):
self.robot_pos = [msg.pose.position.x, msg.pose.position.y]
quat = (msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w)
_, _, yaw = tf.transformations.euler_from_quaternion(quat)
self.robot_yaw = yaw
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 cb_goal(self, p):
self.goal = [p.pose.position.x, p.pose.position.y]
if self.robot_pos is None:
rospy.logwarn("%s : no robot pose" % rospy.get_name())
return
# self.robot_yaw = self.robot_yaw + np.pi/2
goal_distance = self.get_distance(self.robot_pos, self.goal)
goal_angle = self.get_goal_angle(self.robot_yaw, self.robot_pos,
self.goal)
pos_output, ang_output = self.control(goal_distance, goal_angle)
cmd_msg = Twist()
cmd_msg.linear.x = self.cmd_constarin(pos_output)
cmd_msg.angular.z = self.cmd_constarin(ang_output)
self.pub_cmd.publish(cmd_msg)
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 initialize_PID(self):
self.pos_control.setSampleTime(1)
self.ang_control.setSampleTime(1)
self.pos_control.SetPoint = 0.0
self.ang_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)
# 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 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 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
class Robot_PID():
def __init__(self):
self.node_name = rospy.get_name()
self.motor_mode = rospy.get_param("~motor_mode", 0)
rospy.loginfo("Motor Mode: " + str(self.motor_mode))
self.small_angle_thres = 0.35
self.angle_mag = 1 / 2.5
self.dis4constV = 5. # Distance for constant velocity
self.alpha_p = 1.2
self.alpha_a = 1.3
self.pos_ctrl_max = 3
self.pos_ctrl_min = 0.0
# self.alpha_v = 1.0
# self.pos_station_max = 0.8
# self.pos_station_min = -0.8
self.cmd_ctrl_max = 2.5
self.cmd_ctrl_min = -2.5
self.station_keeping_dis = 3.5 # meters
self.frame_id = 'map'
self.goal = None
self.pid_parent = ["pos", "ang", "pos_bridge", "ang_bridge"]
self.pid_child = ["kp", "ki", "kd"]
rospack = rospkg.RosPack()
self.pid_param_path = os.path.join(rospack.get_path('asv_config'),
"pid/pid.yaml")
# if self.motor_mode == 0:
# self.pid_param_path = os.path.join(rospack.get_path('asv_config'), "pid/pid_0.yaml")
# elif self.motor_mode == 1:
# self.pid_param_path = os.path.join(rospack.get_path('asv_config'), "pid/pid_1.yaml")
self.pid_param = None
with open(self.pid_param_path, 'r') as file:
self.pid_param = yaml.safe_load(file)
rospy.loginfo("[%s] Initializing " % (self.node_name))
# self.alpha_srv = rospy.Service("alphaV", SetValue, self.alpha_cb)
self.param_srv = rospy.Service("param", SetString, self.param_cb)
self.pub_cmd = rospy.Publisher("cmd_drive", MotorCmd, queue_size=1)
rospy.Subscriber('robot_goal',
RobotGoal,
self.robot_goal_cb,
queue_size=1,
buff_size=2**24)
self.pub_goal = rospy.Publisher("goal_point", Marker, queue_size=1)
self.pos_control = PID_control("Position")
self.ang_control = PID_control("Angular")
self.pos_bridge_control = PID_control("Position_Bridge")
self.ang_bridge_control = PID_control("Angular_Bridge")
self.set_pid_param()
# self.ang_station_control = PID_control("Angular_station")
# self.pos_station_control = PID_control("Position_station")
# 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 robot_goal_cb(self, msg):
self.goal = [msg.goal.position.x, msg.goal.position.y]
self.robot_position = [msg.robot.position.x, msg.robot.position.y]
if msg.goal.position.x == msg.robot.position.x and msg.goal.position.y == msg.robot.position.y:
# if station keeping
cmd_msg = MotorCmd()
cmd_msg.right = 0
cmd_msg.left = 0
cmd_msg.horizontal = 0
self.pub_cmd.publish(cmd_msg)
self.publish_goal(self.goal)
return
quat = (msg.robot.orientation.x,\
msg.robot.orientation.y,\
msg.robot.orientation.z,\
msg.robot.orientation.w)
_, _, yaw = tf.transformations.euler_from_quaternion(quat)
if len(self.goal) == 0 or len(
self.robot_position
) == 0: # if the robot hasn't recieve any goal
return
#yaw = yaw + np.pi/2
goal_distance = self.get_distance(self.robot_position, self.goal)
goal_angle = self.get_goal_angle(yaw, self.robot_position, self.goal)
pos_output, ang_output = 0, 0
if goal_distance < self.station_keeping_dis:
# rospy.loginfo("Station Keeping")
# pos_output, ang_output = self.station_keeping(goal_distance, goal_angle)
if (msg.mode.data == "bridge"):
if (abs(goal_angle) < self.small_angle_thres):
neg = (goal_angle < 0)
goal_angle = self.small_angle_thres * (
(abs(goal_angle) / self.small_angle_thres)**
self.angle_mag)
if neg:
goal_angle = -goal_angle
pos_output, ang_output = self.bridge_control(
goal_distance, goal_angle)
else:
pos_output, ang_output = self.control(goal_distance,
goal_angle)
else:
if (msg.mode.data == "bridge"):
if (abs(goal_angle) < self.small_angle_thres):
neg = (goal_angle < 0)
goal_angle = self.small_angle_thres * (
(abs(goal_angle) / self.small_angle_thres) *
self.angle_mag)
if neg:
goal_angle = -goal_angle
pos_output, ang_output = self.bridge_control(
goal_distance, goal_angle)
else:
pos_output, ang_output = self.control(goal_distance,
goal_angle)
cmd_msg = MotorCmd()
if self.motor_mode == 0: # three motors mode
if not msg.only_angle.data: # for navigation
cmd_msg.right = self.cmd_constarin(pos_output + ang_output)
cmd_msg.left = self.cmd_constarin(pos_output - ang_output)
else: # if only for rotation, instead of moving forward
cmd_msg.right = self.cmd_constarin(ang_output)
cmd_msg.left = self.cmd_constarin(ang_output)
elif self.motor_mode == 1: # four motors mode
if not msg.only_angle.data: # for navigation
cmd_msg.right = self.cmd_constarin(pos_output)
cmd_msg.left = self.cmd_constarin(pos_output)
cmd_msg.horizontal = self.cmd_constarin(ang_output)
else: # if only for rotation, instead of moving forward
cmd_msg.horizontal = self.cmd_constarin(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.alpha_p * (-self.pos_control.output / self.dis4constV))
# -1 = -180/180 < output/180 < 180/180 = 1
ang_output = self.alpha_a * (self.ang_control.output / 180.)
return pos_output, ang_output
def bridge_control(self, goal_distance, goal_angle):
self.pos_bridge_control.update(goal_distance)
self.ang_bridge_control.update(goal_angle)
# pos_output will always be positive
pos_output = self.pos_constrain(
self.alpha_p * (-self.pos_bridge_control.output / self.dis4constV))
# -1 = -180/180 < output/180 < 180/180 = 1
ang_output = self.alpha_a * (self.ang_bridge_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.alpha_p *
(-self.pos_station_control.output / self.dis4constV))
# -1 = -180/180 < output/180 < 180/180 = 1
ang_output = self.alpha_a * (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 param_cb(self, req):
s = req.str
ss = s.split('/')
string_valid = False
if len(ss) == 3:
if ss[0] in self.pid_parent:
if ss[1] in self.pid_child:
try:
string_valid = True
self.pid_param[ss[0]][ss[1]] = float(ss[2])
with open(self.pid_param_path, "w") as file:
yaml.dump(self.pid_param, file)
self.set_pid_param()
except:
pass
res = SetStringResponse()
if string_valid:
res.success = True
else:
res.success = False
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
self.pos_bridge_control.setSampleTime(1)
self.ang_bridge_control.setSampleTime(1)
# self.pos_station_control.setSampleTime(1)
# self.ang_station_control.setSampleTime(1)
self.pos_bridge_control.SetPoint = 0.0
self.ang_bridge_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 = 1.5
marker.scale.y = 1.5
marker.scale.z = 1.5
marker.color.a = 1.0
marker.color.g = 1.0
marker.color.r = 1.0
self.pub_goal.publish(marker)
def set_pid_param(self):
self.pos_control.setKp(self.pid_param['pos']['kp'])
self.pos_control.setKi(self.pid_param['pos']['ki'])
self.pos_control.setKd(self.pid_param['pos']['kd'])
self.ang_control.setKp(self.pid_param['ang']['kp'])
self.ang_control.setKi(self.pid_param['ang']['ki'])
self.ang_control.setKd(self.pid_param['ang']['kd'])
self.pos_bridge_control.setKp(self.pid_param['pos_bridge']['kp'])
self.pos_bridge_control.setKi(self.pid_param['pos_bridge']['ki'])
self.pos_bridge_control.setKd(self.pid_param['pos_bridge']['kd'])
self.ang_bridge_control.setKp(self.pid_param['ang_bridge']['kp'])
self.ang_bridge_control.setKi(self.pid_param['ang_bridge']['ki'])
self.ang_bridge_control.setKd(self.pid_param['ang_bridge']['kd'])
rospy.loginfo("PID parameters:")
print("pos: ", self.pid_param['pos'])
print("ang: ", self.pid_param['ang'])
print("pos_bridge: ", self.pid_param['pos_bridge'])
print("ang_bridge: ", self.pid_param['ang_bridge'])
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
class JoyMapper(object):
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " % (self.node_name))
# Publications
self.pub_motor_cmd = rospy.Publisher("motor_cmd",
Motor4Cmd,
queue_size=1)
# Subscriptions
self.sub_cmd_drive = rospy.Subscriber("cmd_drive",
VelocityVector,
self.cbCmd,
queue_size=1)
self.sub_joy = rospy.Subscriber("joy", Joy, self.cbJoy, queue_size=1)
self.sub_imu = rospy.Subscriber("imu/data",
Imu,
self.cb_imu,
queue_size=1)
#PID
self.ang_control = PID_control("joy_stick_Angular")
self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb,
"joy_stick_Angular")
self.ang_control.setSampleTime(0.1)
self.ang_control.SetPoint = 0.0
#parameter
self.differential_constrain = rospy.get_param("differential_constrain",
0.2)
#varibles
self.emergencyStop = False
self.autoMode = False
self.rotate = 0
self.motor_msg = Motor4Cmd()
self.last_msg = Motor4Cmd()
self.vector_msg = VelocityVector()
self.imu_msg = Imu()
self.boat_angle = 0
self.dest_angle = 0
self.last_msg.lf = 0
self.last_msg.lr = 0
self.last_msg.rf = 0
self.last_msg.rr = 0
self.motor_stop()
#timer
self.timer = rospy.Timer(rospy.Duration(0.05), self.cb_publish)
def cb_publish(self, event):
if self.emergencyStop:
self.motor_stop()
self.pub_motor_cmd.publish(self.motor_msg)
else: #low pass filter
if not self.autoMode:
if self.rotate > 0:
self.dest_angle += 5
elif self.rotate < 0:
self.dest_angle -= 5
self.dest_angle = self.angle_range(self.dest_angle)
angular_input = self.angle_range(self.boat_angle -
self.dest_angle)
angular_output = self.control(angular_input)
print("angular out %f" % angular_output)
self.vector_msg.angular = angular_output
self.motor_msg = self.vector_to_cmd(self.vector_msg)
self.motor_msg = self.msg_constrain(self.motor_msg)
self.last_msg.lf = self.low_pass_filter(self.motor_msg.lf,
self.last_msg.lf)
self.last_msg.lr = self.low_pass_filter(self.motor_msg.lr,
self.last_msg.lr)
self.last_msg.rf = self.low_pass_filter(self.motor_msg.rf,
self.last_msg.rf)
self.last_msg.rr = self.low_pass_filter(self.motor_msg.rr,
self.last_msg.rr)
self.pub_motor_cmd.publish(self.last_msg)
def low_pass_filter(self, current, last):
current = last + max(min(current - last, self.differential_constrain),
-self.differential_constrain)
return current
def control(self, head_angle):
self.ang_control.update(head_angle)
ang_output = self.ang_control.output / (-180.)
return ang_output
def cbCmd(self, cmd_msg):
if not self.emergencyStop and self.autoMode:
msg = self.vector_to_cmd(cmd_msg)
self.motor_msg = self.msg_constrain(msg)
def cbJoy(self, joy_msg):
self.processButtons(joy_msg)
if not self.emergencyStop and not self.autoMode:
self.joy = joy_msg
self.vector_msg = VelocityVector()
self.vector_msg.x = self.joy.axes[0] * -1
self.vector_msg.y = self.joy.axes[1]
if self.joy.axes[3] > 0:
self.rotate = 1
elif self.joy.axes[3] < 0:
self.rotate = -1
else:
self.rotate = 0
self.vector_msg.angular = 0
def cb_imu(self, msg):
self.imu_msg = msg
quat = (msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w)
_, _, yaw = tf.transformations.euler_from_quaternion(quat)
self.boat_angle = np.degrees(yaw)
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 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 motor_stop(self):
self.motor_msg.lf = 0
self.motor_msg.lr = 0
self.motor_msg.rf = 0
self.motor_msg.rr = 0
def vector_to_cmd(self, vec):
motor4msg = Motor4Cmd()
motor4msg.lf = max(min(vec.y + vec.x, 1), -1) + vec.angular
motor4msg.lr = max(min(vec.y - vec.x, 1), -1) + vec.angular
motor4msg.rf = max(min(vec.y - vec.x, 1), -1) - vec.angular
motor4msg.rr = max(min(vec.y + vec.x, 1), -1) - vec.angular
return motor4msg
def msg_constrain(self, msg):
new_msg = Motor4Cmd()
new_msg.lf = max(min(msg.lf, 1), -1)
new_msg.lr = max(min(msg.lr, 1), -1)
new_msg.rf = max(min(msg.rf, 1), -1)
new_msg.rr = max(min(msg.rr, 1), -1)
return new_msg
def processButtons(self, joy_msg):
# Button A
if (joy_msg.buttons[0] == 1):
rospy.loginfo('A button')
# Y button
elif (joy_msg.buttons[3] == 1):
rospy.loginfo('Y button')
# Left back button
elif (joy_msg.buttons[4] == 1):
rospy.loginfo('left back button')
# Right back button
elif (joy_msg.buttons[5] == 1):
rospy.loginfo('right back button')
# Back button
elif (joy_msg.buttons[6] == 1):
rospy.loginfo('back button')
# Start button
elif (joy_msg.buttons[7] == 1):
self.autoMode = not self.autoMode
if self.autoMode:
rospy.loginfo('going auto')
else:
rospy.loginfo('going manual')
# Power/middle button
elif (joy_msg.buttons[8] == 1):
self.emergencyStop = not self.emergencyStop
if self.emergencyStop:
rospy.loginfo('emergency stop activate')
self.motor_stop()
else:
rospy.loginfo('emergency stop release')
# Left joystick button
elif (joy_msg.buttons[9] == 1):
rospy.loginfo('left joystick button')
else:
some_active = sum(joy_msg.buttons) > 0
if some_active:
rospy.loginfo('No binding for joy_msg.buttons = %s' %
str(joy_msg.buttons))
def on_shutdown(self):
self.motor_stop()
self.pub_motor_cmd.publish(self.motor_msg)
rospy.loginfo("shutting down [%s]" % (self.node_name))
