20170508

bool Costmap2D::mapToWorld(double w,double m_p_x,double m_p_y,double &w_p_x,double &w_p_y)

{

if(0 == m_p_x && 0 == m_p_y)

{

w_p_x = origin_x_;

w_p_y = origin_y_;

return true;

}

if(0 == m_p_x && m_p_y > 0)

{

w_p_x = origin_x_ + (hypot(m_p_x, m_p_y)*cos(M_PI/2.0 + w)+0.5)*resolution_;

w_p_y = origin_y_ + (hypot(m_p_x, m_p_y)*sin(M_PI/2.0 + w)+0.5)*resolution_;

}

else if(0 == m_p_x && m_p_y < 0)

{

w_p_x = origin_x_ + (hypot(m_p_x, m_p_y)*cos(3*M_PI/2.0 + w)+0.5)*resolution_;

w_p_y = origin_y_ + (hypot(m_p_x, m_p_y)*sin(3*M_PI/2.0 + w)+0.5)*resolution_;

}

else

{

w_p_x = origin_x_ + (hypot(m_p_x, m_p_y)*cos(atan2(m_p_y, m_p_x)+w)+0.5)*resolution_;

w_p_y = origin_y_ + (hypot(m_p_x, m_p_y)*sin(atan2(m_p_y, m_p_x)+w)+0.5)*resolution_;

}

return true;

}

void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const

{

  wx = origin_x_ + (mx + 0.5) * resolution_;

  wy = origin_y_ + (my + 0.5) * resolution_;

}

bool Costmap2D::worldToMap(double w,double w_p_x,double w_p_y,unsigned int &m_p_x,unsigned int &m_p_y)

{

if(origin_x_ == w_p_x && origin_y_ == w_p_y)

{

m_p_x = 0;

m_p_y = 0;

return true;

}

double m1_p_x = w_p_x - origin_x_;

double m1_p_y = w_p_y - origin_y_;

if(0 == m1_p_x && m1_p_y > 0)

{

m_p_x = (hypot(m1_p_x, m1_p_y)*cos(M_PI/2.0 - w))/resolution_;

m_p_y = (hypot(m1_p_x, m1_p_y)*sin(M_PI/2.0 - w))/resolution_;

}

else if(0 == m1_p_x && m1_p_y < 0)

{

m_p_x = (hypot(m1_p_x, m1_p_y)*cos(3*M_PI/2.0 - w))/resolution_;

m_p_y = (hypot(m1_p_x, m1_p_y)*sin(3*M_PI/2.0 - w))/resolution_;

}

else

{

m_p_x = (hypot(m1_p_x, m1_p_y)*cos(atan2(m1_p_y, m1_p_x) - w))/resolution_;

m_p_y = (hypot(m1_p_x, m1_p_y)*sin(atan2(m1_p_y, m1_p_x) - w))/resolution_;

}

return true;

}