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