自动回充问题求解答

[cams]

1.发射区域
说明中分了4块区域左侧区域，右侧区域，中间区域，区域内的值为4，1，2，3（重合区），请问下，4这个值是广角红外发射的值，会出现在左侧区域，右侧区域，中间区域中吗？
2.位姿计算
机器人的坐标原点在哪里？
下面代码的变换过程能用图表解释下吗？
void DockController::caculatePose3()
{
float theta1, theta2, x, y, theta;

tf::Quaternion q1(mPose1_.orientation.x, mPose1_.orientation.y, mPose1_.orientation.z, mPose1_.orientation.w);
tf::Matrix3x3 m1(q1);
double roll, pitch, yaw;
m1.getRPY(roll, pitch, yaw);
theta1 = yaw;

tf::Quaternion q2(mPose1_.orientation.x, mPose1_.orientation.y, mPose1_.orientation.z, mPose1_.orientation.w);
tf::Matrix3x3 m2(q2);
m2.getRPY(roll, pitch, yaw);
theta2 = yaw;

x = (mPose1_.position.x + mPose2_.position.x) / 2.0f;
y = (mPose1_.position.y + mPose2_.position.y) / 2.0f;
theta = (theta1 + theta2) / 2.0f;

mPose3_[0] = x - back_distance_ * cos(theta);
mPose3_[1] = y - back_distance_ * sin(theta);
mPose3_[2] = theta;
//ROS_DEBUG("theta12  %f  %f x1 y1 %f %f x2 y2 %f %f ", theta1, theta2, mPose1_.position.x, mPose1_.position.y,
//            mPose2_.position.x, mPose2_.position.y);

3.}

3. 0.3;//多少度？
   if (mdock__referenss_position_ == DOCK_POSITION::left_center)
   {
   //右转
   current_vel.angular.z = -0.3;//多少度？
   }