go_along_circle.py 文件源码

def go_to(self, radius, angular, mode):
        # ??????????
        symbol_y,symbol_w = self.MODE[mode]
        ang_has_moved = 0.0
        self.reach_goal = False
        move_vel = g_msgs.Twist()
        start_yaw = self.get_position.get_robot_current_w()
        while not rospy.is_shutdown() and self.reach_goal != True:
            current_yaw = self.get_position.get_robot_current_w()
            ang_has_moved += abs(abs(current_yaw) - abs(start_yaw))
            start_yaw = current_yaw
            if abs(ang_has_moved - abs(angular)) <= self.stop_tolerance:
                self.reach_goal = True
                break
            move_vel.linear.y = self.move_speed*symbol_y
            # ???????, ???? dw*dt = dt*atan2(dv*dt/2.0, radius)
            move_vel.angular.z = self.rate*atan2(self.move_speed/self.rate,radius)*symbol_w
            print ang_has_moved
            self.move_cmd_pub.publish(move_vel)
            self.R.sleep()
        self.brake()

        print ang_has_moved