gazebo map on opencv

asked 2020-11-26 06:30:27 -0500

slm gravatar image

Hello, how can I draw the gazebo world which is seen by the laser sensor of the turtlebot3 being controlled by the keyboard into opencv mat?

The coordinates must be calculated according to the base reference system taking the viewpoint into account. Namely odom messages's quertion must be converted to euler and these measures must be taken into account. And scan message must be used for drawing.

I did something but I'm not sure about the calculated coordinates and I can't think of the way of drawing whole area. In addition the keyboard code commands do not work. My related codes:

struct Quaternion quaternion;
quaternion.x=msg->pose.pose.orientation.x;
quaternion.y=msg->pose.pose.orientation.y;
quaternion.z=msg->pose.pose.orientation.z;
quaternion.w=msg->pose.pose.orientation.w;
eulerAngles=ToEulerAngles(quaternion);
double x=  msg->pose.pose.position.x*cos(eulerAngles.yaw)- msg->pose.pose.position.y*sin(eulerAngles.yaw);
double y=  msg->pose.pose.position.x*sin(eulerAngles.yaw)+ msg->pose.pose.position.y*cos(eulerAngles.yaw);  
newLaserPosition= cv::Point(x,y);

..................................................................................

for(int i=0; i < msg->ranges.size(); i++)
{
    float angle = msg->angle_min+i*msg->angle_increment;
    float norm_range = (msg->ranges.at(i)/msg->range_max)*(WINDOW/2);
    x = (int)(WINDOW/2+newLaserPosition.x+norm_range*cos(angle));
    y = (int)(WINDOW/2+newLaserPosition.y+norm_range*sin(angle));
    cv::circle( image, cv::Point(x,y), 1, cv::Scalar( 0, 0, 255), cv::FILLED, cv::LINE_8);
}

Could you suggest anything else? Thanks.

edit retag flag offensive close merge delete