def poseCB(self, p):
self.robot_pose = p.pose.pose
self.robot_pose.position.x, self.robot_pose.position.y = self.projection(
p.pose.pose.position.x, p.pose.pose.position.y)
self.robot_pose.orientation.x = -p.pose.pose.orientation.x
self.robot_pose.orientation.y = -p.pose.pose.orientation.y
self.robot_pose.orientation.z = -p.pose.pose.orientation.z
self.robot_pose.orientation = quat_rot(self.robot_pose.orientation, 0,
0, 90)
self.pose_get = True
self.orentation_get = True
if not self.goal_set:
return
self.distance = math.sqrt(
(self.goal.x - self.robot_pose.position.x)**2 +
(self.goal.y - self.robot_pose.position.y)**2)
self.z_dist = self.robot_pose.position.z - self.goal.z
robot_euler = tf.transformations.euler_from_quaternion(
(self.robot_pose.orientation.x, self.robot_pose.orientation.y,
self.robot_pose.orientation.z, self.robot_pose.orientation.w))
self.roll = 0
self.pitch = math.atan2(
self.goal.z - self.robot_pose.position.z,
math.sqrt((self.goal.x - self.robot_pose.position.x)**2 +
(self.goal.y -
self.robot_pose.position.y)**2)) - robot_euler[1]
self.yaw = math.atan2(
self.goal.y - self.robot_pose.position.y,
self.goal.x - self.robot_pose.position.x) - robot_euler[2]
self.roll = fit_in_rad(self.roll)
self.pitch = fit_in_rad(self.pitch)
self.yaw = fit_in_rad(self.yaw)
def cmdCB(self, cmd):
cmd_parts = cmd.data.split(',')
s = cmd_parts[0]
pos = self.robot_pos
robot_euler = tf.transformations.euler_from_quaternion((pos.orientation.x, pos.orientation.y, pos.orientation.z, pos.orientation.w))
robot_th = robot_euler[2] # 2 * math.atan2(odo.pose.pose.orientation.z,odo.pose.pose.orientation.w)
if s == "fwd":
dis = float(cmd_parts[1])
self.target_pos.position.x = pos.position.x + dis * math.cos(robot_th)
self.target_pos.position.y = pos.position.y + dis * math.sin(robot_th)
self.robot_state = self.FORWARDING
self.finished.data = False
self.state = self.RUNNING
elif s == "turn":
a = math.radians( float(cmd_parts[1]) )
target_th = robot_th + a
target_th = fit_in_rad(target_th)
target_quat = tf.transformations.quaternion_from_euler(robot_euler[0], robot_euler[1], target_th)
self.target_pos.orientation.x = target_quat[0]
self.target_pos.orientation.y = target_quat[1]
self.target_pos.orientation.z = target_quat[2]
self.target_pos.orientation.w = target_quat[3]
self.robot_state = self.TURNING
self.finished.data = False
self.state = self.RUNNING
def Start(self):
while not rospy.is_shutdown():
if self.state != self.RUNNING:
self.rate.sleep()
continue
# Calculate relative position
pos = self.robot_pos
distance = math.sqrt( (self.target_pos.position.x-pos.position.x)**2 + (self.target_pos.position.y-pos.position.y)**2 )
z_dist = self.target_pos.position.y - pos.position.y
robot_euler = tf.transformations.euler_from_quaternion((pos.orientation.x, pos.orientation.y, pos.orientation.z, pos.orientation.w))
target_euler = tf.transformations.euler_from_quaternion((self.target_pos.orientation.x, self.target_pos.orientation.y, self.target_pos.orientation.z, self.target_pos.orientation.w))
angle = fit_in_rad( math.atan2((self.target_pos.position.y-pos.position.y),(self.target_pos.position.x-pos.position.x)) - robot_euler[2])
if math.fabs(angle) < math.pi/2:
fwd_dir = 1.0
else:
fwd_dir = -1.0
roll = fit_in_rad(target_euler[0] - robot_euler[0])
pitch = fit_in_rad(target_euler[1] - robot_euler[1])
yaw = fit_in_rad(target_euler[2] - robot_euler[2])
# When the robot is turning
if self.robot_state == self.TURNING:
# Stop linear movements
self.vel.linear.x = 0
self.vel.linear.y = 0
self.vel.linear.z = 0
# Turn if the robot has DOF
if self.rx_config:
self.vel.angular.x = self.Accelerate(self.vel.angular.x, self.K_ROLL * roll, self.ACC_R/self.freq)
else:
self.vel.angular.x = 0
if self.ry_config:
self.vel.angular.y = self.Accelerate(self.vel.angular.y, self.K_PITCH * pitch, self.ACC_R/self.freq)
else:
self.vel.angular.y = 0
if self.rz_config:
self.vel.angular.z = self.Accelerate(self.vel.angular.z, self.K_YAW * yaw, self.ACC_R/self.freq)
else:
self.vel.angular.z = 0
# Check whether turning process is finished
finished_turning = True
if self.rx_config and math.fabs(roll) > self.TURNING_THRES:
finished_turning = False
if self.ry_config and math.fabs(pitch) > self.TURNING_THRES:
finished_turning = False
if self.rz_config and math.fabs(yaw) > self.TURNING_THRES:
finished_turning = False
# If turning is finished
if finished_turning:
if not self.finished.data:
self.finished.data = True
self.finished_pub.publish(self.finished)
self.StopRobot()
self.robot_state = self.STOP
self.state = self.STOP
# When the robot is moving forwarding
elif self.robot_state == self.FORWARDING:
# TODO: movement in x-y plane should be optimised
self.vel.linear.x = self.Accelerate(self.vel.linear.x, fwd_dir * self.K_RHO * distance, self.ACC/self.freq)
# If the robot is able to fly
if self.z_config:
self.vel.linear.z = self.Accelerate(self.vel.linear.z, self.K_RHO * z_dist, self.ACC_R/self.freq)
# Correct the orenitation if the robot can
if self.rx_config:
self.vel.angular.x = self.Accelerate(self.vel.angular.x, self.K_ROLL * roll, self.ACC_R/self.freq)
else:
self.vel.angular.x = 0
if self.ry_config:
self.vel.angular.y = self.Accelerate(self.vel.angular.y, self.K_PITCH * pitch, self.ACC_R/self.freq)
else:
self.vel.angular.y = 0
if self.rz_config:
self.vel.angular.z = self.Accelerate(self.vel.angular.z, self.K_YAW * yaw, self.ACC_R/self.freq)
else:
self.vel.angular.z = 0
# Check whether the target is reached
finished_forwarding = True
if math.fabs(distance) > self.FORWARDING_THRES:
finished_forwarding = False
if self.z_config and math.fabs(z_dist) > self.FLYING_THRES:
finished_forwarding = False
# When reach the target, stop the robot and wait for new command
if finished_forwarding:
if not self.finished.data:
self.finished.data = True
self.finished_pub.publish(self.finished)
self.StopRobot()
self.robot_state = self.IDLE
self.state = self.STOP
# When the cmd is not known
else:
self.robot_state = self.STOP
self.StopRobot()
self.state = self.STOP
# Fit the velocity into the limited range
self.vel.linear.x = self.LimitRange(self.vel.linear.x, self.VEL_MAX_LIN)
self.vel.linear.y = self.LimitRange(self.vel.linear.y, self.VEL_MAX_LIN)
self.vel.linear.z = self.LimitRange(self.vel.linear.z, self.VEL_MAX_LIN)
self.vel.angular.x = self.LimitRange(self.vel.angular.x, self.VEL_MAX_ANG)
self.vel.angular.y = self.LimitRange(self.vel.angular.y, self.VEL_MAX_ANG)
self.vel.angular.z = self.LimitRange(self.vel.angular.z, self.VEL_MAX_ANG)
self.vel_pub.publish(self.vel)
self.rate.sleep()
def Start(self):
while not rospy.is_shutdown():
if self.finished.data:
self.rate.sleep()
continue
# Calculate relative position
pos = self.robot_pos
distance = math.sqrt(
(self.target_pos.position.x - pos.position.x)**2 +
(self.target_pos.position.y - pos.position.y)**2)
robot_euler = tf.transformations.euler_from_quaternion(
(pos.orientation.x, pos.orientation.y, pos.orientation.z,
pos.orientation.w))
target_euler = tf.transformations.euler_from_quaternion(
(self.target_pos.orientation.x, self.target_pos.orientation.y,
self.target_pos.orientation.z, self.target_pos.orientation.w))
angle = fit_in_rad(
math.atan2((self.target_pos.position.y - pos.position.y),
(self.target_pos.position.x - pos.position.x)) -
robot_euler[2])
if math.fabs(angle) < math.pi / 2:
fwd_dir = 1.0
else:
fwd_dir = -1.0
yaw = fit_in_rad(target_euler[2] - robot_euler[2])
if yaw > 0:
turn_dir = 1.0
else:
turn_dir = -1.0
# print "distance: " + str(distance)
# print "yaw: " + str(yaw)
# When the robot is turning
if self.robot_state == self.TURNING:
# Stop linear movements
self.vel.linear.x = 0
self.vel.angular.z = turn_dir * self.VEL_ANG
# Check whether turning process is finished
finished_turning = True
if math.fabs(yaw) > self.TURNING_THRES:
finished_turning = False
# If turning is finished
if finished_turning:
if not self.finished.data:
self.finished.data = True
self.finished_pub.publish(self.finished)
self.StopRobot()
self.robot_state = self.IDLE
# When the robot is moving forwarding
elif self.robot_state == self.FORWARDING:
self.vel.linear.x = fwd_dir * self.VEL_LIN
self.vel.angular.z = 0
# Check whether the target is reached
finished_forwarding = True
if math.fabs(distance) > self.FORWARDING_THRES:
finished_forwarding = False
# When reach the target, stop the robot and wait for new command
if finished_forwarding:
if not self.finished.data:
self.finished.data = True
self.finished_pub.publish(self.finished)
self.StopRobot()
self.robot_state = self.IDLE
# When the cmd is not known
else:
self.robot_state = self.IDLE
self.StopRobot()
self.finished.data = True
self.finished_pub.publish(self.finished)
self.vel_pub.publish(self.vel)
self.rate.sleep()