ros:两向量或位姿间最小旋转角度及旋向

该代码片段展示了一个函数,用于计算从当前位姿到目标位姿所需的最小旋转角度和方向。通过使用ROS消息格式和tf库,它将姿态信息转换为欧拉角并确定旋转操作。

描述:

输入:目标位姿,实时更新的当前位姿
功能:判断当前位姿----->目标位姿的最小旋转角度(rotation_angle)和旋转方向(rotation_dir)

代码

void target_reached(const geometry_msgs::PoseStamped &msg_target,const geometry_msgs::PoseStamped &msg_now)
  {
   
   
  
  //确定当前yaw和目标yaw(弧度制,0--->-pi;-pi---->0)
  tf::Quaternion quat,quat_n
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值