Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

New global_mapper_node #2

Open
wants to merge 1 commit into
base: vardhan
Choose a base branch
from
Open

New global_mapper_node #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions global_mapper/launch/global.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
<?xml version="1.0"?>
<launch>

<!-- Front LiDAR -->
<node name="global_mapper" pkg="global_mapper" type="global_mapper_node" output="screen" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_to_tv_broadcaster" args="4.5 4 0 1.57079 3.14159 0 base_link top_view"/>
</launch>
90 changes: 45 additions & 45 deletions global_mapper/src/global_mapper_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,23 +1,25 @@
#include <ros/ros.h>
#include <stdlib.h>
#include <nav_msgs/OccupancyGrid.h>
#include "cv_bridge/cv_bridge.h"
#include "image_transport/image_transport.h"
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>

#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;

Expand Down Expand Up @@ -46,13 +48,13 @@ class real_mapper
tfListener = new tf2_ros::TransformListener(tfBuffer);
pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/real_map", 1);
sub1_ = nh_.subscribe<sensor_msgs::LaserScan>("/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");
Expand All @@ -61,22 +63,24 @@ class real_mapper
if((laser_angle>-M_PI/2 && laser_angle<M_PI) || scan_msg->ranges[i] < scan_msg->range_min || scan_msg->ranges[i] > scan_msg->range_max)
continue;
for(float range_current = 0.0; range_current<scan_msg->ranges[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);
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] = 100;
}
}
Expand All @@ -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<gs.rows;i++)
{
{
for(int j=0;j<gs.cols;j++)
{
// point_cam.point.x = (gs.rows-1-i)/image_scale*cellResolution;
// point_cam.point.y = j/image_scale*cellResolution;
// 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);
x=(gs.rows-1-i)/image_scale*cellResolution;
y=j/image_scale*cellResolution;
if ((0 < y*real_map_width + x) && (y*real_map_width + x < real_map_width*real_map_height)){
// if ((bool)gs.at<uchar>(j, i) && (x != i && y != j)){
if (gs.at<int>(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)<real_map_width && ((gs.rows-1-i)/(image_scale*cellResolution))<real_map_height)
// if (gs.at<uchar>(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<uchar>(i, j)==0)
{
// real_map.data[real_map_height*y + x] = 0;
}
else
{
real_map.data[real_map_height*y + x] = 100;
}
}
}
}
}

Expand All @@ -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();
Expand Down