fixed bug working [slow]

iKrishneel · Jun 17, 2020 · 743bcfd · 743bcfd
1 parent aee972d
commit 743bcfd
Show file tree

Hide file tree

Showing 6 changed files with 161 additions and 50 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -22,14 +22,16 @@ find_package(PCL 1.11 REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(OpenMP REQUIRED)
 find_package(pcl_conversions REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(octomap REQUIRED)
 find_package(octomap_msgs REQUIRED)
-find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
 find_package(tf2_msgs REQUIRED)
 
 link_directories(${PCL_LIBRARY_DIRS})
@@ -50,6 +52,8 @@ ament_target_dependencies(octomap_server2
   std_msgs
   nav_msgs
   visualization_msgs
+  geometry_msgs
+  std_srvs
   octomap
   octomap_msgs
   message_filters
@@ -58,7 +62,7 @@ ament_target_dependencies(octomap_server2
   tf2
   )
 
-target_link_libraries(octomap_server2 ${PCL_LIBRARIES})
+target_link_libraries(octomap_server2 ${PCL_LIBRARIES} ${OCTOMAP_LIBRARIES})
 
 rclcpp_components_register_node(octomap_server2
   PLUGIN "octomap_server::OctomapServer"

diff --git a/include/octomap_server2/conversions.h b/include/octomap_server2/conversions.h
@@ -42,6 +42,7 @@
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
 
@@ -90,7 +91,7 @@ namespace octomap {
 
     /// Conversion from octomap::point3d to tf2::Point
     static inline geometry_msgs::msg::Point pointOctomapToTf(
-        const point3d &octomapPt) {        
+        const point3d &octomapPt) {
         geometry_msgs::msg::Point pt;
         pt.x = octomapPt.x();
         pt.y = octomapPt.y();
@@ -102,8 +103,13 @@ namespace octomap {
     static inline octomap::point3d pointTfToOctomap(
         const geometry_msgs::msg::Point& ptTf){
         return point3d(ptTf.x, ptTf.y, ptTf.z);
+    }    
+
+    static inline octomap::point3d pointTfToOctomap(
+        const geometry_msgs::msg::Vector3& ptTf){
+        return point3d(ptTf.x, ptTf.y, ptTf.z);
     }
-
+    
     /// Conversion from octomap Quaternion to tf2::Quaternion
     static inline tf2::Quaternion quaternionOctomapToTf(
         const octomath::Quaternion& octomapQ){

diff --git a/include/octomap_server2/octomap_server.hpp b/include/octomap_server2/octomap_server.hpp
@@ -10,6 +10,8 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <std_msgs/msg/color_rgba.hpp>
 #include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_srvs/srv/empty.hpp>
@@ -31,6 +33,7 @@
 #include <tf2/buffer_core.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
+#include <tf2_ros/create_timer_ros.h>
 #include <tf2_ros/message_filter.h>
 
 #include <octomap_msgs/conversions.h>
@@ -203,7 +206,7 @@ namespace octomap_server {
         virtual void publishAll(const rclcpp::Time &);
 
         virtual void insertScan(
-            const geometry_msgs::msg::Point& sensorOrigin,
+            const geometry_msgs::msg::Vector3  &sensorOrigin,
             const PCLPointCloud& ground,
             const PCLPointCloud& nonground);
 

diff --git a/include/octomap_server2/transforms.hpp b/include/octomap_server2/transforms.hpp
@@ -8,14 +8,18 @@
 #include <tf2/buffer_core.h>
 #include <tf2/transform_datatypes.h>
 
+#include <geometry_msgs/msg/transform_stamped.hpp>
 
 namespace pcl_ros {
 
-  /** \brief Obtain the transformation matrix from TF into an Eigen form
-    * \param bt the TF transformation
-    * \param out_mat the Eigen transformation
-    */
-  void transformAsMatrix(const tf2::Transform&, Eigen::Matrix4f &);
+    /** \brief Obtain the transformation matrix from TF into an Eigen form
+     * \param bt the TF transformation
+     * \param out_mat the Eigen transformation
+     */
+    void transformAsMatrix(
+        const tf2::Transform &, Eigen::Matrix4f &);
+    Eigen::Matrix4f transformAsMatrix(
+        const geometry_msgs::msg::TransformStamped &);
 }
 
 #endif // PCL_ROS_TRANSFORMS_H_