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)
def send_motor_cmd(self, event):
if self.heading is None:
return
mcd_msg = UsvDrive()
mcd_msg.header.stamp = rospy.Time.now()
speed = self.heading.speed * math.sin(self.heading.phi)
difference = self.heading.speed * math.cos(self.heading.phi)
mcd_msg.left = max(min(speed - difference, 1), -1)
mcd_msg.right = max(min(speed + difference, 1), -1)
self.pub_motor_cmd.publish(mcd_msg)
def send_motor_cmd(self, event):
if self.motor_cmd is not None:
motor_msg = None
if self.sim:
from duckiepond_vehicle.msg import UsvDrive
motor_msg = UsvDrive()
else:
motor_msg = MotorCmd()
motor_msg.left = self.motor_cmd[0]
motor_msg.right = self.motor_cmd[1]
motor_msg.header.stamp = rospy.Time.now()
self.pub_motion.publish(motor_msg)
print("Motor cmd = ", motor_msg.left, motor_msg.right)
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 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 send_motor_cmd(self, event):
motor_msg = None
if self.sim:
from duckiepond_vehicle.msg import UsvDrive
motor_msg = UsvDrive()
else:
motor_msg = MotorCmd()
motor_msg.header.stamp = rospy.Time.now()
motor_msg.left = 0
motor_msg.right = 0
if self.wamv_desired_heading is None or self.wamv_desired_rudder is None or self.wamv_desired_speed is None:
pass
elif abs(self.wamv_desired_rudder - 0.0) < 0.001 and abs(
self.wamv_desired_thruster_l
) < 5.0 and self.wamv_desired_speed < 0.1:
pass
else:
if abs(self.wamv_desired_thruster_l) < 5.0:
motor_msg.left = self.linear_speed
motor_msg.right = self.linear_speed
if abs(self.wamv_desired_speed - 0) < 0.002 and abs(
self.wamv_desired_thruster_l) > 5.0:
motor_msg.left = motor_msg.left + self.wamv_desired_thruster_l * 4 * self.angular_speed
motor_msg.right = motor_msg.right + self.wamv_desired_thruster_r * 4 * self.angular_speed
else:
motor_msg.left = motor_msg.left + self.wamv_desired_rudder * self.angular_speed * self.wamv_desired_speed
motor_msg.right = motor_msg.right - self.wamv_desired_rudder * self.angular_speed * self.wamv_desired_speed
if motor_msg.left >= 0.9:
motor_msg.left = 0.9
elif motor_msg.left <= -0.9:
motor_msg.left = -0.9
if motor_msg.right >= 0.9:
motor_msg.right = 0.9
elif motor_msg.right <= -0.9:
motor_msg.right = -0.9
self.pub_motion.publish(motor_msg)
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%d" % i) for i in range(4)
]
self.angu_pid = [
PID_control("angular_control%d" % i) for i in range(4)
]
self.dis_server = Server(dis_PIDConfig, self.cb_dis_pid,
"distance_control")
self.ang_server = Server(ang_PIDConfig, self.cb_ang_pid,
"angular_control")
for i in range(4):
self.dis_pid[i].setSampleTime(0.1)
self.angu_pid[i].setSampleTime(0.1)
self.dis_pid[i].SetPoint = 0
self.angu_pid[i].SetPoint = 0
#setup publisher
self.pub_v1 = rospy.Publisher("/boat1/cmd_drive",
UsvDrive,
queue_size=1)
self.sub_p3d1 = rospy.Subscriber("/boat1/p3d_odom",
Odometry,
self.cb_boat1_odom,
queue_size=1)
self.pub_v2 = rospy.Publisher("/boat2/cmd_drive",
UsvDrive,
queue_size=1)
self.sub_p3d1 = rospy.Subscriber("/boat2/p3d_odom",
Odometry,
self.cb_boat2_odom,
queue_size=1)
self.pub_v3 = rospy.Publisher("/boat3/cmd_drive",
UsvDrive,
queue_size=1)
self.sub_p3d1 = rospy.Subscriber("/boat3/p3d_odom",
Odometry,
self.cb_boat3_odom,
queue_size=1)
self.pub_v4 = rospy.Publisher("/boat4/cmd_drive",
UsvDrive,
queue_size=1)
self.sub_p3d1 = rospy.Subscriber("/boat4/p3d_odom",
Odometry,
self.cb_boat4_odom,
queue_size=1)
#initiallize boat status
self.boat_odom = [Odometry() for i in range(4)]
self.cmd_drive = [UsvDrive() for i in range(4)]
self.yaw = [0 for i in range(4)]
#initiallize HRVO environment
self.ws_model = dict()
#robot radius
self.ws_model['robot_radius'] = 3
self.ws_model['circular_obstacles'] = []
#rectangular boundary, format [x,y,width/2,heigth/2]
self.ws_model['boundary'] = []
self.pin1 = [7.5, 7.5]
self.pin2 = [-7.5, 7.5]
self.pin3 = [-7.5, -7.5]
self.pin4 = [7.5, -7.5]
self.position = []
self.goal = [self.pin3, self.pin4, self.pin1, self.pin2]
#print(self.position)
#print(self.goal)
self.velocity = [[0, 0] for i in range(4)]
self.velocity_detect = [[0, 0] for i in range(4)]
self.v_max = [1 for i in range(4)]
#timer
self.timer = rospy.Timer(rospy.Duration(0.2), self.cb_hrvo)
def control_cmd_drive(self, dis, angle):
cmd = UsvDrive()
cmd.left = max(min(dis - angle, 1), -1)
cmd.right = max(min(dis + angle, 1), -1)
return cmd