【python+ROS+路径规划】七、rviz角度和mpc角度问题

rviz角度

在ros中，当我们在rviz里面获取机器人坐标点的时候会获取到机器人的角度，也就是四元数。我们将其转换成角度：

def getTarTheta(self):
        self.TarPose = rospy.wait_for_message("/move_base_simple/goal", PoseStamped,timeout=None)
        self.tar_quaternions_x = self.TarPose.pose.orientation.x
        self.tar_quaternions_y = self.TarPose.pose.orientation.y
        self.tar_quaternions_z = self.TarPose.pose.orientation.z
        self.tar_quaternions_w = self.TarPose.pose.orientation.w
        
        #这里有个转换
        (_, _, self.y) = transformations.euler_from_quaternion([self.tar_quaternions_x, self.tar_quaternions_y, self.tar_quaternions_z, self.tar_quaternions_w])

其实rviz里面的角度范围是（-180，180]

mpc生成的角度

但是mpc算出来的角度不是点的角度（-180，180]，比如如果我想让机器人从180这个角度逆时针转90度，那么rviz中获取的两个角度就是一个180，一个-90。但是如果使用这个角度进行轨迹跟踪，从180一下子变到-179，再变到-90，这肯定是有问题的。理想情况下，mpc输入的角度应该是180，270。
所以这就需要有个获取角度并改变角度的事情。

角度更改代码

我的解决方案如下：
检测到在rviz中获取的两次相邻的点的角度如果突然差的很大，那么就对后一个加上360度。

    def getNowPose(self):
        IniPose = rospy.wait_for_message("/amcl_pose", PoseWithCovarianceStamped,timeout=None)
        #获取点坐标
        now_x = IniPose.pose.pose.position.x
        now_y = IniPose.pose.pose.position.y

        #获取四元数
        init_quaternions_x = IniPose.pose.pose.orientation.x
        init_quaternions_y = IniPose.pose.pose.orientation.y
        init_quaternions_z = IniPose.pose.pose.orientation.z
        init_quaternions_w = IniPose.pose.pose.orientation.w
        #转换成角度
        (_, _,now_theta) = transformations.euler_from_quaternion([init_quaternions_x,init_quaternions_y,init_quaternions_z,init_quaternions_w])
        #这里角度的转换有些许问题

        return now_x,now_y,now_theta


#这个函数是获取机器人在rviz中坐标点的函数
now_x,now_y,now_theta = self.getNowPose()

#mpc的角度和rviz获取的角度不一样

theta_history[1] = now_theta
if abs(theta_history[0]-theta_history[1])>3.2 :
    opt_theta = 2*math.pi + now_theta
else:
    opt_theta = now_theta
theta_history[0] = opt_theta
theta_history[1] = 0
#print(now_theta,opt_theta)