Skip to content

Commit f58c8d6

Browse files
idryanovidryanov
committed
initial commit
0 parents  commit f58c8d6

File tree

113 files changed

+10588
-0
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

113 files changed

+10588
-0
lines changed

‎CMakeLists.txt

+17
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
cmake_minimum_required(VERSION 2.4.6)
2+
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3+
4+
# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
5+
# directories (or patterns, but directories should suffice) that should
6+
# be excluded from the distro. This is not the place to put things that
7+
# should be ignored everywhere, like "build" directories; that happens in
8+
# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
9+
# ready for inclusion in a distro.
10+
#
11+
# This list is combined with the list in rosbuild/rosbuild.cmake. Note
12+
# that CMake 2.6 may be required to ensure that the two lists are combined
13+
# properly. CMake 2.4 seems to have unpredictable scoping rules for such
14+
# variables.
15+
#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
16+
17+
rosbuild_make_distribution(0.1.0)

‎README

+42
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
---------------------------------------------------------
2+
CCNY RGBD
3+
---------------------------------------------------------
4+
Ivan Dryanovski [email protected]
5+
CCNY Robotics Lab 2011
6+
http://robotics.ccny.cuny.edu/
7+
8+
1. DETAILS:
9+
10+
This code is in an experimental stage. It provides several classes capable of
11+
providing real-time visual odometry for a hand-held RGB-D camera, and a map
12+
server which accumulates an octree-based map.
13+
14+
2. INSTALLING:
15+
16+
First, install the dependencies:
17+
18+
sudo apt-get install ros-fuerte-openni-camera
19+
sudo apt-get install ros-fuerte-openni-launch
20+
sudo apt-get install libgsl0-dev
21+
22+
Next, compile the stack:
23+
24+
rosmake ccny_rgbd
25+
26+
27+
3. USAGE
28+
29+
Connect your RGB-D camera and launch the Openni device.
30+
31+
roslaunch ccny_openni_launch openni.launch
32+
33+
Next, launch the VO:
34+
35+
roslaunch ccny_rgbd_vo rgbdvo.launch
36+
37+
Finally, launch rviz.
38+
39+
rosrun rviz rviz
40+
41+
For convenience, you can load the ccny_rgbd_vo/launch/rviz.cfg file.
42+

‎ccny_3dmap_server/CMakeLists.txt

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
cmake_minimum_required(VERSION 2.4.6)
2+
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3+
4+
# Set the build type. Options are:
5+
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
6+
# Debug : w/ debug symbols, w/o optimization
7+
# Release : w/o debug symbols, w/ optimization
8+
# RelWithDebInfo : w/ debug symbols, w/ optimization
9+
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
10+
set(ROS_BUILD_TYPE MinSizeRel)
11+
12+
rosbuild_init()
13+
14+
#set the default path for built executables to the "bin" directory
15+
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
16+
#set the default path for built libraries to the "lib" directory
17+
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
18+
19+
#uncomment if you have defined messages
20+
#rosbuild_genmsg()
21+
#uncomment if you have defined services
22+
rosbuild_gensrv()
23+
24+
rosbuild_add_executable(map_server_node src/map_server_node.cpp
25+
src/map_server.cpp)

‎ccny_3dmap_server/Makefile

+1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
include $(shell rospack find mk)/cmake.mk

‎ccny_3dmap_server/include/ccny_3dmap_server/map_server.h

+87
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
#ifndef CCNY_3D_MAP_SERVER_MAP_SERVER_H
2+
#define CCNY_3D_MAP_SERVER_MAP_SERVER_H
3+
4+
#include <ros/ros.h>
5+
#include <sensor_msgs/PointCloud2.h>
6+
#include <tf/transform_listener.h>
7+
#include <tf/transform_broadcaster.h>
8+
#include <pcl/point_types.h>
9+
#include <pcl/point_cloud.h>
10+
#include <pcl/filters/voxel_grid.h>
11+
#include <pcl/io/pcd_io.h>
12+
//#include <pcl/io/ply_io.h>
13+
#include <pcl_ros/point_cloud.h>
14+
#include <pcl_ros/transforms.h>
15+
16+
#include <ccny_gicp/octree_pointcloud_storage.h>
17+
#include <ccny_gicp/octree_pointcloud_storage.hpp>
18+
19+
#include "ccny_3dmap_server/Save.h"
20+
21+
namespace ccny_rgbd
22+
{
23+
24+
typedef pcl::PointXYZRGB PointT;
25+
typedef pcl::PointCloud<PointT> PointCloudT;
26+
27+
const std::string sub_topic_ = "/camera/depth_registered/points";
28+
const std::string map_topic_ = "/map";
29+
30+
class MapServer
31+
{
32+
public:
33+
34+
typedef pcl::octree::OctreePointCloudStorage<PointT> Octree;
35+
36+
MapServer(ros::NodeHandle nh, ros::NodeHandle nh_private);
37+
virtual ~MapServer();
38+
39+
void pointCloudCallback(const PointCloudT::ConstPtr& cloud_in_ptr);
40+
41+
bool saveSrvCallback(ccny_3dmap_server::Save::Request& request,
42+
ccny_3dmap_server::Save::Response& response);
43+
44+
45+
private:
46+
47+
// **** ROS-related
48+
49+
ros::NodeHandle nh_;
50+
ros::NodeHandle nh_private_;
51+
52+
ros::Subscriber point_cloud_subscriber_;
53+
ros::Publisher map_pub_;
54+
55+
tf::TransformListener tf_listener_;
56+
tf::TransformBroadcaster tf_broadcaster_;
57+
58+
ros::ServiceServer save_service_;
59+
60+
// **** parameters
61+
62+
std::string fixed_frame_;
63+
double resolution_;
64+
double max_range_;
65+
66+
// **** variables
67+
68+
unsigned int cloud_count_;
69+
70+
boost::mutex mutex_;
71+
72+
Octree * octree_;
73+
PointCloudT::Ptr map_;
74+
75+
// **** private functions
76+
77+
void initParams();
78+
79+
void publishMap();
80+
81+
void filterCloud(const PointCloudT::ConstPtr& cloud_in_ptr,
82+
const PointCloudT::Ptr& data_ptr);
83+
};
84+
85+
} //namespace ccny_rgbd
86+
87+
#endif // CCNY_3D_MAP_SERVER_MAP_SERVER_H

‎ccny_3dmap_server/launch/mapper.launch

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
<launch>
2+
<node pkg="ccny_3dmap_server" type="map_server_node" name="map_server"
3+
output="screen">
4+
5+
<param name="resolution" value="0.02"/>
6+
<param name="fixed_frame" value="world"/>
7+
<param name="max_range" value="3"/>
8+
9+
</node>
10+
11+
</launch>
12+

‎ccny_3dmap_server/launch/mapper_o.launch

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
<launch>
2+
<node pkg="ccny_3dmap_server" type="map_server_node" name="map_server"
3+
output="screen">
4+
5+
<param name="resolution" value="0.02"/>
6+
<param name="fixed_frame" value="odom"/>
7+
<param name="max_range" value="3"/>
8+
9+
</node>
10+
11+
</launch>
12+

‎ccny_3dmap_server/manifest.xml

+20
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
<package>
2+
<description brief="ccny_3dmap_server">
3+
4+
ccny_3dmap_server
5+
6+
</description>
7+
<author>Ivan Dryanovski</author>
8+
<license>BSD</license>
9+
<review status="unreviewed" notes=""/>
10+
<url>http://ros.org/wiki/ccny_3dmap_server</url>
11+
<depend package="roscpp"/>
12+
<depend package="sensor_msgs"/>
13+
<depend package="tf"/>
14+
<depend package="pcl"/>
15+
<depend package="pcl_ros"/>
16+
<depend package="ccny_gicp"/>
17+
18+
</package>
19+
20+

‎ccny_3dmap_server/src/map_server.cpp

+142
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,142 @@
1+
#include "ccny_3dmap_server/map_server.h"
2+
3+
namespace ccny_rgbd {
4+
5+
MapServer::MapServer(ros::NodeHandle nh, ros::NodeHandle nh_private):
6+
nh_(nh),
7+
nh_private_(nh_private),
8+
resolution_(0.01),
9+
max_range_(4.0)
10+
{
11+
ROS_INFO("Starting Map Server");
12+
13+
initParams();
14+
15+
map_ = boost::make_shared<PointCloudT>();
16+
map_->header.frame_id = fixed_frame_;
17+
18+
octree_ = new Octree(resolution_);
19+
octree_->setInputCloud(map_);
20+
21+
// **** publishers
22+
23+
map_pub_ = nh_.advertise<PointCloudT>(
24+
map_topic_, 1);
25+
26+
// **** subscribers
27+
28+
point_cloud_subscriber_ = nh_.subscribe<PointCloudT>(
29+
sub_topic_, 1, &MapServer::pointCloudCallback, this);
30+
31+
// **** services
32+
33+
save_service_ = nh_.advertiseService(
34+
"save_map", &MapServer::saveSrvCallback, this);
35+
}
36+
37+
MapServer::~MapServer()
38+
{
39+
ROS_INFO("Destroying Map Server");
40+
41+
}
42+
43+
void MapServer::initParams()
44+
{
45+
46+
if (!nh_private_.getParam ("resolution", resolution_))
47+
resolution_ = 0.01;
48+
if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
49+
fixed_frame_ = "world";
50+
if (!nh_private_.getParam ("max_range", max_range_))
51+
max_range_ = 4.0;
52+
53+
}
54+
55+
void MapServer::pointCloudCallback(const PointCloudT::ConstPtr& cloud_in_ptr)
56+
{
57+
struct timeval start, end;
58+
gettimeofday(&start, NULL);
59+
60+
// **** get fixed frame to camera tf ************************************************
61+
62+
tf::StampedTransform f2c_tf;
63+
64+
try
65+
{
66+
tf_listener_.waitForTransform (
67+
fixed_frame_, cloud_in_ptr->header.frame_id, cloud_in_ptr->header.stamp, ros::Duration(1.0));
68+
tf_listener_.lookupTransform (
69+
fixed_frame_, cloud_in_ptr->header.frame_id, cloud_in_ptr->header.stamp, f2c_tf);
70+
}
71+
catch (tf::TransformException ex)
72+
{
73+
ROS_WARN("TF unavailable %s", ex.what());
74+
return;
75+
}
76+
77+
tf::Transform f2c = f2c_tf;
78+
79+
// **** create data cloud ***************************************************
80+
81+
PointCloudT::Ptr data_ptr = boost::make_shared<PointCloudT>();
82+
filterCloud(cloud_in_ptr, data_ptr);
83+
84+
// **** transform to best guess for world frame ******************************
85+
86+
pcl_ros::transformPointCloud (*data_ptr, *data_ptr, f2c);
87+
data_ptr->header.frame_id = fixed_frame_;
88+
89+
// **** add to map *********************************************************
90+
91+
boost::mutex::scoped_lock(mutex_);
92+
93+
for (unsigned int i = 0; i < data_ptr->points.size(); ++i)
94+
{
95+
int index;
96+
bool replaced = octree_->addPointWithReplacement(data_ptr->points[i], map_, index);
97+
}
98+
map_->header.stamp = cloud_in_ptr->header.stamp;
99+
100+
publishMap();
101+
102+
gettimeofday(&end, NULL);
103+
}
104+
105+
void MapServer::publishMap()
106+
{
107+
boost::mutex::scoped_lock(mutex_);
108+
map_pub_.publish(map_);
109+
}
110+
111+
void MapServer::filterCloud(const PointCloudT::ConstPtr& cloud_in_ptr,
112+
const PointCloudT::Ptr& data_ptr)
113+
{
114+
for (unsigned int i = 0; i < cloud_in_ptr->points.size(); ++i)
115+
{
116+
float z = cloud_in_ptr->points[i].z;
117+
118+
if (!isnan(z) && z < max_range_)
119+
data_ptr->points.push_back(cloud_in_ptr->points[i]);
120+
}
121+
122+
data_ptr->is_dense = true;
123+
data_ptr->width = data_ptr->points.size();
124+
data_ptr->height = 1;
125+
data_ptr->header = cloud_in_ptr->header;
126+
}
127+
128+
bool MapServer::saveSrvCallback(
129+
ccny_3dmap_server::Save::Request& request,
130+
ccny_3dmap_server::Save::Response& response)
131+
{
132+
ROS_INFO("Saving to %s...", request.filename.c_str());
133+
ROS_INFO("Map has %dK points", map_->points.size()/1024);
134+
135+
pcl::io::savePCDFileBinary<PointT>(request.filename, *map_);
136+
//pcl::io::savePLYFile<PointT>(request.filename + ".ply", *map_);
137+
138+
ROS_INFO("Save successful");
139+
return true;
140+
}
141+
142+
} // namespace ccny_rgbd

‎ccny_3dmap_server/src/map_server_node.cpp

+11
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
#include "ccny_3dmap_server/map_server.h"
2+
3+
int main(int argc, char** argv)
4+
{
5+
ros::init(argc, argv, "MapServer");
6+
ros::NodeHandle nh;
7+
ros::NodeHandle nh_private("~");
8+
ccny_rgbd::MapServer ms(nh, nh_private);
9+
ros::spin();
10+
return 0;
11+
}

‎ccny_3dmap_server/srv/Save.srv

+2
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
string filename
2+
---

0 commit comments

Comments
 (0)