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)