new publisher

CMakeLists.txt

@@ -138,20 +138,22 @@ include_directories(
 # add_library(${PROJECT_NAME}
 #   src/${PROJECT_NAME}/wolf_ros.cpp
 # )
+add_library(publisher_${PROJECT_NAME})
+add_library(subscriber_${PROJECT_NAME})
+
+target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_laser_map.cpp)
+target_sources(subscriber_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/subscriber_laser2d.cpp)
+
 if (falkolib_FOUND)
 	message("Found Falkolib. Compiling publisher_falko.")
-	add_library(subscriber_${PROJECT_NAME}
-	  			src/subscriber_laser2d.cpp)
-	add_library(publisher_${PROJECT_NAME} 
-				src/publisher_laser_map.cpp
-	  			src/publisher_falko.cpp)
-else()
-	message("Falkolib NOT FOUND.")
-	add_library(subscriber_${PROJECT_NAME}
-	  			src/subscriber_laser2d.cpp)
-	add_library(publisher_${PROJECT_NAME} 
-				src/publisher_laser_map.cpp)
-endif()		
+	target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_falko.cpp)
+endif()
+
+find_file(ICP wolflaser_INCLUDE_DIRS laser/processor/processor_odom_icp.h)
+if (NOT ICP_NOTFOUND)
+	message("Found 'processor_odom_icp.h'. Compiling publisher_odom_icp.")
+	target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_odom_icp.cpp)
+endif ()
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries

include/publisher_odom_icp.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef PUBLISHER_ODOM_ICP_H
+#define PUBLISHER_ODOM_ICP_H
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/problem/problem.h"
+#include "publisher.h"
+#include "laser/processor/processor_odom_icp.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
+
+namespace wolf
+{
+
+class PublisherOdomIcp: public Publisher
+{
+    protected:
+
+        ProcessorOdomIcpPtr processor_odom_icp_;
+        geometry_msgs::PoseStamped msg_;
+
+    public:
+
+        PublisherOdomIcp(const std::string& _unique_name,
+                         const ParamsServer& _server,
+                         const ProblemPtr _problem);
+        WOLF_PUBLISHER_CREATE(PublisherOdomIcp);
+
+        virtual ~PublisherOdomIcp(){};
+
+        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+
+        void publishDerived() override;
+
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherOdomIcp)
+}
+
+#endif

src/publisher_odom_icp.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "publisher_odom_icp.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+
+namespace wolf
+{
+
+PublisherOdomIcp::PublisherOdomIcp(const std::string& _unique_name,
+                                   const ParamsServer& _server,
+                                   const ProblemPtr _problem) :
+        Publisher(_unique_name, _server, _problem),
+        processor_odom_icp_(nullptr)
+{
+    auto processor_name = _server.getParam<std::string>(prefix_ + "/processor_name");
+    
+    // search the processor
+    for (auto sen : _problem->getHardware()->getSensorList())
+    {
+        for (auto proc : sen->getProcessorList())
+        {
+            if (proc->getName() == processor_name)
+            {
+                processor_odom_icp_ = std::dynamic_pointer_cast<ProcessorOdomIcp>(proc);
+                if (not processor_odom_icp_)
+                    throw std::runtime_error("PublisherOdomIcp requires a processor of type 'ProcessorOdomIcp'.");
+                break;
+            }
+        }
+        if (processor_odom_icp_)
+            break;
+    }
+
+    // frame_id of the msg
+    msg_.header.frame_id = getParamWithDefault<std::string>(_server,
+                                                            prefix_ + "/frame_id",
+                                                            "/base_link");
+}
+
+void PublisherOdomIcp::initialize(ros::NodeHandle& nh, const std::string& topic)
+{
+    publisher_ = nh.advertise<geometry_msgs::PoseStamped>(topic, 1);
+}
+
+void PublisherOdomIcp::publishDerived()
+{
+    if (publisher_.getNumSubscribers() == 0 )
+        return;
+
+    Eigen::Vector3d pose_origin_last = processor_odom_icp_->getOriginLastTransform();
+
+    auto q_origin_last = Eigen::Quaterniond(Eigen::AngleAxisd(pose_origin_last(2),
+                                            Eigen::Vector3d::UnitZ()));
+
+    msg_.pose.position.x    = pose_origin_last(0);
+    msg_.pose.position.y    = pose_origin_last(1);
+    msg_.pose.position.z    = 0;
+    msg_.pose.orientation.x = q_origin_last.x();
+    msg_.pose.orientation.y = q_origin_last.y();
+    msg_.pose.orientation.z = q_origin_last.z();
+    msg_.pose.orientation.w = q_origin_last.w();
+
+    //Publish
+    publisher_.publish(msg_);
+}
+
+}