diff --git a/crosbot_ogmbicp/CMakeLists.txt b/crosbot_ogmbicp/CMakeLists.txt index 7d388f67dbd50a9301a54d4e14bd668c2b981cea..0d846249a628a0d0123716a512fbfeed0f6931a4 100644 --- a/crosbot_ogmbicp/CMakeLists.txt +++ b/crosbot_ogmbicp/CMakeLists.txt @@ -66,15 +66,15 @@ include_directories(include # ) ## Declare a cpp executable -# add_executable(crosbot_ogmbicp_node src/crosbot_ogmbicp_node.cpp) +add_executable(crosbot_ogmbicp src/main.cpp src/OgmbicpNode.cpp src/Ogmbicp.cpp) ## Add dependencies to the executable # add_dependencies(crosbot_ogmbicp_node ${PROJECT_NAME}) ## Specify libraries to link a library or executable target against -# target_link_libraries(crosbot_ogmbicp_node -# ${catkin_LIBRARIES} -# ) + target_link_libraries(crosbot_ogmbicp + ${catkin_LIBRARIES} + ) ############# ## Install ## @@ -95,11 +95,11 @@ include_directories(include # ) ## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" + PATTERN ".svn" EXCLUDE + ) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES @@ -107,6 +107,12 @@ include_directories(include # # myfile2 # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + FILES_MATCHING PATTERN "*.launch" + PATTERN ".svn" EXCLUDE + ) + ############# ## Testing ## diff --git a/crosbot_ogmbicp/include/crosbot_ogmbicp/Ogmbicp.hpp b/crosbot_ogmbicp/include/crosbot_ogmbicp/Ogmbicp.hpp new file mode 100644 index 0000000000000000000000000000000000000000..453895fd6e1bd59eed58ed68a1cf04220277bd6b --- /dev/null +++ b/crosbot_ogmbicp/include/crosbot_ogmbicp/Ogmbicp.hpp @@ -0,0 +1,51 @@ +/* + * Ogmbicp.hpp + * + * Created on: 12/9/2013 + * Author: adrianr + */ + +#ifndef OGMBICP_HPP_ +#define OGMBICP_HPP_ + +#include +#include +#include +#include +#include +#include + +using namespace crosbot; +using namespace std; + +class Ogmbicp { +public: + /* + * Initialise the position tracker + */ + void initialise(ros::NodeHandle &nh); + + /* + * Start the position tracker. After this is called, the position + * tracker is ready to take scans + */ + void start(); + + /* + * Stop the position tracker + */ + void stop(); + + /* + * Called for processing the first scan + */ + void initialiseTrack(Pose sensorPose, PointCloudPtr cloud); + + /* + * Update the position tracker with the lastest scan + */ + void updateTrack(Pose sensorPose, PointCloudPtr cloud); + +}; + +#endif diff --git a/crosbot_ogmbicp/include/crosbot_ogmbicp/OgmbicpNode.hpp b/crosbot_ogmbicp/include/crosbot_ogmbicp/OgmbicpNode.hpp new file mode 100644 index 0000000000000000000000000000000000000000..5592b8584ad04c86db10e1c7652de053008d68a3 --- /dev/null +++ b/crosbot_ogmbicp/include/crosbot_ogmbicp/OgmbicpNode.hpp @@ -0,0 +1,71 @@ +/* + * OgmbicpNode.hpp + * + * Created on: 12/9/2013 + * Author: adrianr + */ + + +#ifndef OGMBICPNODE_HPP_ +#define OGMBICPNODE_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include + +#define DEFAULT_ICPFRAME "/icp" +#define DEFAULT_BASEFRAME "/base_link" +#define DEFAULT_ODOMFRAME "" +#define DEFAULT_MAXWAIT4TRANSFORM 2.0 + +using namespace std; + +class OgmbicpNode { +public: + + OgmbicpNode(Ogmbicp&); + + /* + * Initialises the ogmbicp node + */ + void initialise(ros::NodeHandle& nh); + + /* + * Shuts down the ogmbicp node + */ + void shutdown(); + +private: + + /* + * ROS config params for position tracking + */ + string icp_frame, base_frame, odom_frame; + string scan_sub; + + + /* + * ROS connections + */ + ros::Subscriber scanSubscriber; + tf::TransformListener tfListener; + + Ogmbicp &pos_tracker; + //Is it the initial scan? + bool isInit; + + /* + * Main callback for position tracker. Processes a new scan + */ + void callbackScan(const sensor_msgs::LaserScanConstPtr& lastestScan); + +}; + + +#endif diff --git a/crosbot_ogmbicp/include/crosbot_ogmbicp/pointgrid.hpp b/crosbot_ogmbicp/include/crosbot_ogmbicp/pointgrid.hpp deleted file mode 100644 index 68f05823a2d631d75f31bc9d0e5c16435a1c4c71..0000000000000000000000000000000000000000 --- a/crosbot_ogmbicp/include/crosbot_ogmbicp/pointgrid.hpp +++ /dev/null @@ -1,55 +0,0 @@ -/* - * pointgrid.hpp - * - * Created on: 28/02/2013 - * Author: mmcgill - */ - -#ifndef CROSBOT_POINTGRID_HPP_ -#define CROSBOT_POINTGRID_HPP_ - -namespace crosbot { - -namespace ogmbicp { - -class PointGrid { -public: - class Cell : public HandledObject { - public: - Cell() {} - }; - typedef Handle CellPtr; - - uint32_t width, height; - double resolution; - std::vector< std::vector < CellPtr > > cells; - Pose origin; - - PointGrid(uint32_t width, uint32_t height, double resolution) : - width(width), height(height), resolution(resolution), - origin(-width * resolution / 2.0, -height * resolution / 2.0, 0) - { - if (width > 0 && height > 0) { - cells.resize(height); - for (uint32_t r = 0; r < height; ++r) { - cells[r].resize(width); - for (uint32_t c = 0; c < width; ++c) { - cells[r][c] = new Cell(); - } - } - } - } - - void centerOn(Point p); -protected: - void shift(int x, int y); -}; - -} - -} // namespace crosbot - - - - -#endif /* CROSBOT_POINTGRID_HPP_ */ diff --git a/crosbot_ogmbicp/launch/crosbot_ogmbicp.launch b/crosbot_ogmbicp/launch/crosbot_ogmbicp.launch new file mode 100644 index 0000000000000000000000000000000000000000..c2c757d0d105ecaf69384a3c4a2faa4d66392325 --- /dev/null +++ b/crosbot_ogmbicp/launch/crosbot_ogmbicp.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/crosbot_ogmbicp/src/Ogmbicp.cpp b/crosbot_ogmbicp/src/Ogmbicp.cpp new file mode 100644 index 0000000000000000000000000000000000000000..80f8e989a6c9274f91a58eadd5624fd77bcfa537 --- /dev/null +++ b/crosbot_ogmbicp/src/Ogmbicp.cpp @@ -0,0 +1,28 @@ +/* + * Ogmbicp.cpp + * + * Created on: 16/9/2013 + * Author: adrianr + */ + +#include + +void Ogmbicp::initialise(ros::NodeHandle &nh) { +} + +void Ogmbicp::start() { + cout << "starting ogmbicp" << endl; +} + +void Ogmbicp::stop() { + cout << "stopping ogmbicp" << endl; + +} + +void Ogmbicp::initialiseTrack(Pose sensorPose, PointCloudPtr cloud) { + cout << "ogmbicp: initialise track" << endl; +} + +void Ogmbicp::updateTrack(Pose sensorPose, PointCloudPtr cloud) { + cout << "ogmbicp: update track" << endl; +} diff --git a/crosbot_ogmbicp/src/OgmbicpNode.cpp b/crosbot_ogmbicp/src/OgmbicpNode.cpp new file mode 100644 index 0000000000000000000000000000000000000000..dee6016da8843778e0c26ae948c97822e4052e69 --- /dev/null +++ b/crosbot_ogmbicp/src/OgmbicpNode.cpp @@ -0,0 +1,71 @@ +/* + * OgmbicpNode.cpp + * + * Created on: 12/9/2013 + * Author: adrianr + */ + +#include + +using namespace std; +using namespace crosbot; + +OgmbicpNode::OgmbicpNode(Ogmbicp &posTracker): icp_frame(DEFAULT_ICPFRAME), + base_frame(DEFAULT_BASEFRAME), + odom_frame(DEFAULT_ODOMFRAME), + pos_tracker(posTracker) +{ + pos_tracker = posTracker; + isInit = false; +} + +void OgmbicpNode::initialise(ros::NodeHandle &nh) { + ros::NodeHandle paramNH("~"); + paramNH.param("icp_frame", icp_frame, DEFAULT_ICPFRAME); + paramNH.param("base_frame", base_frame, DEFAULT_BASEFRAME); + paramNH.param("odom_frame", odom_frame, DEFAULT_ODOMFRAME); + paramNH.param("scan_sub", scan_sub, "scan"); + + pos_tracker.initialise(nh); + pos_tracker.start(); + + scanSubscriber = nh.subscribe(scan_sub, 1, &OgmbicpNode::callbackScan, this); +} + +void OgmbicpNode::shutdown() { + scanSubscriber.shutdown(); + pos_tracker.stop(); + +} + +void OgmbicpNode::callbackScan(const sensor_msgs::LaserScanConstPtr& latestScan) { + Pose odomPose, sensorPose; + tf::StampedTransform laser2Base, base2Odom; + bool haveOdometry = odom_frame != ""; + try { + tfListener.waitForTransform(base_frame, latestScan->header.frame_id, + latestScan->header.stamp, ros::Duration(1, 0)); + tfListener.lookupTransform(base_frame, + latestScan->header.frame_id, latestScan->header.stamp, laser2Base); + sensorPose = laser2Base; + + if (haveOdometry) { + tfListener.waitForTransform(odom_frame, base_frame, latestScan->header.stamp, ros::Duration(1, 0)); + tfListener.lookupTransform(odom_frame, base_frame, + latestScan->header.stamp, base2Odom); + odomPose = base2Odom; + } + } catch (tf::TransformException& ex) { + fprintf(stderr, "ogmbicp: Error getting transform. (%s) (%d.%d)\n", ex.what(), + latestScan->header.stamp.sec, latestScan->header.stamp.nsec); + return; + } + PointCloudPtr cloud = new PointCloud(base_frame, PointCloud(latestScan, true), sensorPose); + if (!isInit) { + isInit = true; + pos_tracker.initialiseTrack(sensorPose, cloud); + } else { + pos_tracker.updateTrack(sensorPose, cloud); + } +} + diff --git a/crosbot_ogmbicp/src/main.cpp b/crosbot_ogmbicp/src/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..698092ab035643f56678274e82503044d0ce0dcb --- /dev/null +++ b/crosbot_ogmbicp/src/main.cpp @@ -0,0 +1,30 @@ +/* + * main.cpp + * + * Created on: 12/9/2013 + * Author: adrianr + */ + +#include + +#include +#include + +int main(int argc, char **argv) { + ros::init(argc, argv, "ogmbicp"); + + ros::NodeHandle nh; + + Ogmbicp posTracker; + OgmbicpNode node(posTracker); + node.initialise(nh); + + while (ros::ok()) { + ros::spin(); + } + node.shutdown(); + + return 0; +} + +