merge_maps_kinematic.hpp
Go to the documentation of this file.
1 /*
2  * Author
3  * Copyright (c) 2018, Simbe Robotics, Inc.
4  *
5  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
6  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
7  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
8  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
9  *
10  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
11  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
12  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
13  * CONDITIONS.
14  *
15  */
16 
17 /* Author: Steven Macenski */
18 
19 #ifndef SLAM_TOOLBOX_MERGE_MAPS_KINEMATIC_H_
20 #define SLAM_TOOLBOX_MERGE_MAPS_KINEMATIC_H_
21 
22 #include <string>
23 #include <map>
24 #include <memory>
25 #include <vector>
26 #include <sys/stat.h>
27 #include <unistd.h>
28 #include <fstream>
29 #include <boost/thread.hpp>
30 
31 #include "ros/ros.h"
36 #include "tf2_ros/message_filter.h"
39 #include "tf2/utils.h"
40 
41 #include "karto_sdk/Mapper.h"
45 
46 using namespace toolbox_types;
47 
49 {
50 typedef std::vector<karto::LocalizedRangeScanVector>::iterator LocalizedRangeScansVecIt;
51 typedef karto::LocalizedRangeScanVector::iterator LocalizedRangeScansIt;
52 
53 public:
56 
57 private:
58 
59  // setup
60  void setup();
61 
62  // callback
63  bool mergeMapCallback(slam_toolbox_msgs::MergeMaps::Request& req, slam_toolbox_msgs::MergeMaps::Response& resp);
64  bool addSubmapCallback(slam_toolbox_msgs::AddSubmap::Request& req, slam_toolbox_msgs::AddSubmap::Response& resp);
65  void processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
66  void kartoToROSOccupancyGrid(const karto::LocalizedRangeScanVector& scans, nav_msgs::GetMap::Response& map);
67  void transformScan(LocalizedRangeScansIt iter, tf2::Transform& submap_correction);
68 
69  //apply transformation to correct pose
70  karto::Pose2 applyCorrection(const karto::Pose2& pose, const tf2::Transform& submap_correction);
71  karto::Vector2<kt_double> applyCorrection(const karto::Vector2<kt_double>& pose, const tf2::Transform& submap_correction);
72 
73  // ROS-y-ness
75  std::vector<ros::Publisher> sstS_, sstmS_;
77 
78  //karto bookkeeping
79  std::map<std::string, laser_utils::LaserMetadata> lasers_;
80  std::vector<std::unique_ptr<karto::Dataset> > dataset_vec_;
81 
82  // TF
83  std::unique_ptr<tf2_ros::TransformBroadcaster> tfB_;
84 
85  // visualization
86  std::unique_ptr<interactive_markers::InteractiveMarkerServer> interactive_server_;
87 
88  // state
89  std::map<int, Eigen::Vector3d> submap_locations_;
90  std::vector<karto::LocalizedRangeScanVector> scans_vec_;
91  std::map<int, tf2::Transform> submap_marker_transform_;
92  double resolution_;
94 };
95 
96 #endif //SLAM_TOOLBOX_MERGE_MAPS_KINEMATIC_H_
setup
toolbox_types
Definition: toolbox_types.hpp:36
MergeMapsKinematic::LocalizedRangeScansIt
karto::LocalizedRangeScanVector::iterator LocalizedRangeScansIt
Definition: merge_maps_kinematic.hpp:51
utils.h
ros.h
MergeMapsKinematic::ssSubmap_
ros::ServiceServer ssSubmap_
Definition: merge_maps_kinematic.hpp:76
MergeMapsKinematic::resolution_
double resolution_
Definition: merge_maps_kinematic.hpp:92
menu_handler.h
MergeMapsKinematic::tfB_
std::unique_ptr< tf2_ros::TransformBroadcaster > tfB_
Definition: merge_maps_kinematic.hpp:83
MergeMapsKinematic::dataset_vec_
std::vector< std::unique_ptr< karto::Dataset > > dataset_vec_
Definition: merge_maps_kinematic.hpp:80
MergeMapsKinematic::num_submaps_
int num_submaps_
Definition: merge_maps_kinematic.hpp:93
visualization_utils.hpp
transform_broadcaster.h
ros::ServiceServer
MergeMapsKinematic::submap_locations_
std::map< int, Eigen::Vector3d > submap_locations_
Definition: merge_maps_kinematic.hpp:89
MergeMapsKinematic
Definition: merge_maps_kinematic.hpp:48
message_filter.h
karto::LocalizedRangeScanVector
std::vector< LocalizedRangeScan * > LocalizedRangeScanVector
Definition: Karto.h:5879
MergeMapsKinematic::LocalizedRangeScansVecIt
std::vector< karto::LocalizedRangeScanVector >::iterator LocalizedRangeScansVecIt
Definition: merge_maps_kinematic.hpp:50
tf2::Transform
MergeMapsKinematic::sstS_
std::vector< ros::Publisher > sstS_
Definition: merge_maps_kinematic.hpp:75
MergeMapsKinematic::lasers_
std::map< std::string, laser_utils::LaserMetadata > lasers_
Definition: merge_maps_kinematic.hpp:79
toolbox_types.hpp
MergeMapsKinematic::nh_
ros::NodeHandle nh_
Definition: merge_maps_kinematic.hpp:74
karto::Vector2< kt_double >
transform_listener.h
MergeMapsKinematic::submap_marker_transform_
std::map< int, tf2::Transform > submap_marker_transform_
Definition: merge_maps_kinematic.hpp:91
interactive_marker_server.h
tf2_geometry_msgs.h
Mapper.h
karto::Pose2
Definition: Karto.h:2046
MergeMapsKinematic::interactive_server_
std::unique_ptr< interactive_markers::InteractiveMarkerServer > interactive_server_
Definition: merge_maps_kinematic.hpp:86
Matrix3x3.h
MergeMapsKinematic::scans_vec_
std::vector< karto::LocalizedRangeScanVector > scans_vec_
Definition: merge_maps_kinematic.hpp:90
laser_utils.hpp
ros::NodeHandle


slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:55