From 301c36b9ad25f5625f850a61422160b97e95539d Mon Sep 17 00:00:00 2001 From: Nitik Jain <39531001+nitik1998@users.noreply.github.com> Date: Mon, 21 Jan 2019 19:19:17 +0530 Subject: [PATCH] New global_mapper Added mapping capabilities from Vision --- global_mapper/launch/global.launch | 7 ++ global_mapper/src/global_mapper_node.cpp | 90 ++++++++++++------------ 2 files changed, 52 insertions(+), 45 deletions(-) create mode 100644 global_mapper/launch/global.launch diff --git a/global_mapper/launch/global.launch b/global_mapper/launch/global.launch new file mode 100644 index 0000000..97043a1 --- /dev/null +++ b/global_mapper/launch/global.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/global_mapper/src/global_mapper_node.cpp b/global_mapper/src/global_mapper_node.cpp index 4850857..5075a41 100644 --- a/global_mapper/src/global_mapper_node.cpp +++ b/global_mapper/src/global_mapper_node.cpp @@ -1,23 +1,25 @@ #include #include #include -#include "cv_bridge/cv_bridge.h" -#include "image_transport/image_transport.h" -#include "opencv2/core/core.hpp" -#include "opencv2/highgui/highgui.hpp" +#include +#include +#include +#include #include #include #include #include #include +#include +#include -#define cellResolution 0.5 //(meters/cell) -#define real_map_width 200 -#define real_map_height 200 -float map_origin_position[3] = {-200,-200,0}; -#define image_scale 1 //(pixels/meter) -#define mapOriginToImageX 100 //cell no. of pixel -#define mapOriginToImageY 100 // at (x,y)===(columns/2,rows) +#define cellResolution 0.05 //(meters/cell) +#define real_map_width_m 200 +#define real_map_height_m 200 +#define real_map_width (int)(real_map_width_m/cellResolution) +#define real_map_height (int)(real_map_height_m/cellResolution) +float map_origin_position[3] = {-real_map_width_m/2,-real_map_height_m/2,0}; +#define image_scale 160 //(pixels/meter) using namespace std; @@ -46,13 +48,13 @@ class real_mapper tfListener = new tf2_ros::TransformListener(tfBuffer); pub_ = nh_.advertise("/real_map", 1); sub1_ = nh_.subscribe("/scan_1", 1, &real_mapper::scanCallback, this); - sub2_ = nh_.subscribe("/top_view", 100, &real_mapper::imageCallback, this); - ROS_INFO("Started real_ma2ppe2r"); + sub2_ = nh_.subscribe("/top_view", 1, &real_mapper::imageCallback, this); + ROS_INFO("Started real_mapper"); } void scanCallback(const sensor_msgs::LaserScanConstPtr& scan_msg){ - ROS_INFO("Subscribing to scanCallback"); - lidar_to_map = this->tfBuffer.lookupTransform("odom", "laser_1", ros::Time(0)); + + lidar_to_map = tfBuffer.lookupTransform("odom", "laser_front", ros::Time(0)); geometry_msgs::PointStamped point_lidar, point_robot; int x,y; ROS_INFO("moving to outer loop"); @@ -61,14 +63,16 @@ class real_mapper if((laser_angle>-M_PI/2 && laser_angleranges[i] < scan_msg->range_min || scan_msg->ranges[i] > scan_msg->range_max) continue; for(float range_current = 0.0; range_currentranges[i]; range_current+=cellResolution){ - point_lidar.point.x = scan_msg->ranges[i]*cos(laser_angle); - point_lidar.point.y = scan_msg->ranges[i]*sin(laser_angle); + point_lidar.point.x = range_current*cos(laser_angle); + point_lidar.point.y = range_current*sin(laser_angle); point_lidar.point.z = 0.0; tf2::doTransform(point_lidar, point_robot, lidar_to_map); x = (int)((point_robot.point.x - map_origin_position[0])/cellResolution); y = (int)((point_robot.point.y - map_origin_position[1])/cellResolution); - if ((0 < y*real_map_width + x) && (y*real_map_width + x < real_map_width*real_map_height)) + + if ((0 <= y )&&(y < real_map_height)&&(0 <= x)&&(x < real_map_width)) real_map.data[y*real_map_width + x] = 0; + } point_lidar.point.x = scan_msg->ranges[i]*cos(laser_angle); point_lidar.point.y = scan_msg->ranges[i]*sin(laser_angle); @@ -76,7 +80,7 @@ class real_mapper tf2::doTransform(point_lidar, point_robot, lidar_to_map); x = (int)((point_robot.point.x - map_origin_position[0])/cellResolution); y = (int)((point_robot.point.y - map_origin_position[1])/cellResolution); - if ((0 < y*real_map_width + x) && (y*real_map_width + x < real_map_width*real_map_height)) + if ((0 <= y )&&(y < real_map_height)&&(0 <= x)&&(x < real_map_width)) real_map.data[y*real_map_width + x] = 100; } } @@ -89,37 +93,32 @@ class real_mapper } void imageCallback(const sensor_msgs::ImageConstPtr& msg){ - //cam_to_map = this->tfBuffer.lookupTransform("odom", msg->header.frame_id, ros::Time(0)); - //geometry_msgs::PointStamped point_cam, point_robot; + cam_to_map = tfBuffer.lookupTransform("odom", msg->header.frame_id, ros::Time(0)); + geometry_msgs::PointStamped point_cam, point_robot; int x,y; - //bool right_flag = false,left_flag = false; - cv::Mat gs = cv_bridge::toCvShare(msg, "8UC1")->image; - ROS_INFO("moving to image loop %d,%d",gs.rows,gs.cols); + cv::Mat gs = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8)->image; + for(int i=0;i(j, i) && (x != i && y != j)){ - if (gs.at(i,j) == 0) - real_map.data[real_map_height*y + x] = 0; - else - real_map.data[real_map_height*y + x] = 100; - } - //if(j/(image_scale*cellResolution)(i,j) == 0) - // real_map.data[real_map_height*(int)(j/(image_scale*cellResolution)) + (int)((gs.rows-1-i)/(image_scale*cellResolution))] = 0; - // else - // real_map.data[real_map_height*(int)(j/(image_scale*cellResolution)) + (int)((gs.rows-1-i)/(image_scale*cellResolution))] = 100; + point_cam.point.x = j/(double)image_scale; + point_cam.point.y = i/(double)image_scale; + point_cam.point.z = 0.0; + tf2::doTransform(point_cam, point_robot, cam_to_map); + x = (int)((point_robot.point.x - map_origin_position[0])/cellResolution); + y = (int)((point_robot.point.y - map_origin_position[1])/cellResolution); + if ((0 <= y )&&(y < real_map_height)&&(0 <= x)&&(x < real_map_width)){ + if (gs.at(i, j)==0) + { + // real_map.data[real_map_height*y + x] = 0; + } + else + { + real_map.data[real_map_height*y + x] = 100; + } } + } } } @@ -140,6 +139,7 @@ int main(int argc, char** argv) if(loop_count%30==0){ loop_count=0; sub_and_pub.publishMap(); + } ros::spinOnce(); loop_rate.sleep();