#include <stdio.h>
#include <math.h>
#define PI 3.141592
#define ROBOT_ORIGIN_X
/* in
* para 1: costmap原点在world坐标系的 x
* para 2: costmap原点在world坐标系的 y
* para 3: 机器人在world坐标系的 x
* para 4: 机器人在world坐标系的 y
* para 5: 机器人在world坐标系旋转的弧度值
* para 6: p点在world坐标系的 x
* para 7: p点在world坐标系的 y
* out
* para 8: p点在map坐标系的 x
* para 9: p点在map坐标系的 y
1、world_frame 世界坐标系，第一个主坐标系
2、costmap_frame 局部地图坐标系
3、设以robot位置为原点与world坐标系姿态相同的坐标系为r_frame
4、设以robot位置为原点相对world坐标系旋转w的坐标系为rw_frame
*/
#if 1
int world2map(double w_costmap_origin_x,double w_costmap_origin_y,
double w,
double w_p_x,double w_p_y,
double &m_x,double &m_y)
{
if(w_costmap_origin_x == w_p_x && w_costmap_origin_y == w_p_y)
{
m_x = 0;
m_y = 0;
return 0;
}
// 1、计算点p(w_P_x,w_p_y)在map1_frame上的坐标值。
double r_p_x = w_p_x - w_costmap_origin_x;
double r_p_y = w_p_y - w_costmap_origin_y;
// 2、计算P点与机器人相连的线段长
double l_p_r = hypot(r_p_x, r_p_y);
// 3、计算l_p_r与map1_frame x轴正方向上的夹角
double r_frame_a = 0;
if(0 == r_p_x && r_p_y > 0)
{
r_frame_a = PI/2.0;
}
else if(0 == r_p_x && r_p_y < 0)
{
r_frame_a = 3*PI/2.0;
}
else
{
r_frame_a = atan2(r_p_y, r_p_x);
}
// 4、计算l_p_r与map_frame x轴正方向上的夹角
double rw_frame_a = r_frame_a - w;
// 5、计算点p(w_P_x,w_p_y)在mapw_frame上的坐标值。
m_x = l_p_r*cos(rw_frame_a);
m_y = l_p_r*sin(rw_frame_a);
return 0;
}
int map2world(double w_costmap_origin_x,double w_costmap_origin_y,
double w,
double m_p_x,double m_p_y,
double &w_x,double &w_y)
{
// 1、计算点p(w_P_x,w_p_y)在rw_frame上的坐标值。
double rw_p_x = m_p_x - w_costmap_origin_x;
double rw_p_y = m_p_y - w_costmap_origin_y;
// 2、计算P点与机器人相连的线段长
double l_p_r = hypot(m_p_x, m_p_y);
// 3、计算l_p_r与map_frame x轴正方向上的夹角
double map_frame_a = 0;
if(0 == m_p_x && m_p_y > 0)
{
map_frame_a = PI/2.0;
}
else if(0 == m_p_x && m_p_y < 0)
{
map_frame_a = 3*PI/2.0;
}
else
{
map_frame_a = atan2(m_p_x, m_p_x);
}
// 4、计算l_p_r与r_frame x轴正方向上的夹角
double map1_frame_a = map_frame_a + w;
// 5、计算点p(m_P_x,m_p_y)在r_frame上的坐标值。
double map1_p_x = l_p_r*cos(map1_frame_a);
double map1_p_y = l_p_r*sin(map1_frame_a);
// 6、计算点p(w_P_x,w_p_y)在world_frame上的坐标值。
w_x = map1_p_x - w_costmap_origin_x;
w_y = map1_p_x - w_costmap_origin_y;
return 0;
}
#else
int world2map(double w_costmap_origin_x,double w_costmap_origin_y,
double w_robot_x,double w_robot_y,double w,
double w_p_x,double w_p_y,
double &m_x,double &m_y)
{
if(w_robot_x == w_p_x && w_robot_y == w_p_y)
{
m_x = w_p_x;
m_y = w_p_y;
return 0;
}
// 1、计算点p(w_P_x,w_p_y)在r_frame上的坐标值。
double r_p_x = w_p_x - w_robot_x;
double r_p_y = w_p_y - w_robot_y;
// 2、计算P点与机器人相连的线段长
double l_p_r = hypot(r_p_x, r_p_y);
// 3、计算l_p_r与r_frame x轴正方向上的夹角
double r_frame_a = 0;
if(0 == r_p_x && r_p_y > 0)
{
r_frame_a = PI/2.0;
}
else if(0 == r_p_x && r_p_y < 0)
{
r_frame_a = 3*PI/2.0;
}
else
{
r_frame_a = atan2(r_p_y, r_p_x);
}
// 4、计算l_p_r与rw_frame x轴正方向上的夹角
double rw_frame_a = r_frame_a - w;
// 5、计算点p(w_P_x,w_p_y)在rw_frame上的坐标值。
double rw_p_x = l_p_r*cos(rw_frame_a);
double rw_p_y = l_p_r*sin(rw_frame_a);
// 6、计算点p(w_P_x,w_p_y)在costmap_frame上的坐标值。
m_x = rw_p_x - w_costmap_origin_x;
m_y = rw_p_y - w_costmap_origin_y;
return 0;
}
int map2world(double w_costmap_origin_x,double w_costmap_origin_y,
double w_robot_x,double w_robot_y,double w,
double m_p_x,double m_p_y,
double &w_x,double &w_y)
{
// 1、计算点p(w_P_x,w_p_y)在rw_frame上的坐标值。
double rw_p_x = m_p_x - w_costmap_origin_x;
double rw_p_y = m_p_y - w_costmap_origin_y;
// 2、计算P点与机器人相连的线段长
double l_p_r = hypot(rw_p_x, rw_p_y);
// 3、计算l_p_r与rw_frame x轴正方向上的夹角
double rw_frame_a = 0;
if(0 == rw_p_x && rw_p_y > 0)
{
rw_frame_a = PI/2.0;
}
else if(0 == rw_p_x && rw_p_y < 0)
{
rw_frame_a = 3*PI/2.0;
}
else
{
rw_frame_a = atan2(rw_p_x, rw_p_y);
}
// 4、计算l_p_r与r_frame x轴正方向上的夹角
double r_frame_a = rw_frame_a + w;
// 5、计算点p(m_P_x,m_p_y)在r_frame上的坐标值。
double r_p_x = l_p_r*cos(r_frame_a);
double r_p_y = l_p_r*sin(r_frame_a);
// 6、计算点p(w_P_x,w_p_y)在world_frame上的坐标值。
w_x = r_p_x - w_costmap_origin_x;
w_y = r_p_y - w_costmap_origin_y;
return 0;
}
#endif
/* in
* para 1: 机器人在world坐标系的 x
* para 2: 机器人在world坐标系的 y
* para 3: 机器人在world坐标系旋转的弧度值
* para 4: map width在x轴长度
* para 5: map height在y轴长度
* out
* para 6: map在world坐标系的 x
* para 7: map在world坐标系的 y
**/
int get_map_origin(double w_robot_x,double w_robot_y,double w,
double width, double height,
double &map_origin_x,double &map_origin_y)
{
if(0 == width || 0 == height)
{
printf("T error width[%f] height[%f]\n", width, height);
return -1;
}
#if 0
// 1、计算P点与机器人相连的线段长
double l_p_r = hypot(width, height);
// 2、计算l_p_r与r_frame x轴正方向上的夹角
double r_frame_a = atan2(width, height) + PI;
// 3、计算点map_origin在r_frame上的坐标值
double r_map_origin_x = l_p_r*cos(r_frame_a+w);
double r_map_origin_y = l_p_r*sin(r_frame_a+w);
// 4、计算点map_origin在world_frame上的坐标值
map_origin_x = r_map_origin_x + w_robot_x;
map_origin_y = r_map_origin_y + w_robot_y;
#else
map_origin_x = hypot(width, height)*cos(atan2(width, height) + w + PI) + w_robot_x;
map_origin_y = hypot(width, height)*sin(atan2(width, height) + w + PI) + w_robot_y;
#endif
return 0;
}
int main()
{
double map_origin_x, map_origin_y;
double w_robot_x = 0, w_robot_y = 0, w = 0;
double width = 4, height = 4;
// 测试初始值
for(double i=0; i<4; i++)
{
w = i*PI/2.0;
get_map_origin(w_robot_x,w_robot_y,w,width,height,map_origin_x,map_origin_y);
printf("T ---- map_origin_x[%f] map_origin_y[%f]\n", map_origin_x, map_origin_y);
}
double w_p_x = 0;
double w_p_y = 0;
double m_x = 0;
double m_y = 0;
double m_p_x = 0;
double m_p_y = 0;
double w_x;
double w_y;
int world2map(map_origin_x,map_origin_y,w,w_p_x,w_p_y,m_x,m_y);
int map2world(map_origin_x,map_origin_y,w,m_p_x,m_p_y,w_x,w_y);
return 0;
}
