diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 05026060218f95900ef4517df78e099099b80c89..cbaac4732ca625c3b9430327a66f6b3c82ca39fa 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,3 +1,13 @@
+workflow:
+ rules:
+ - if: '$CI_PIPELINE_SOURCE == "web"'
+ - if: $CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS && $CI_PIPELINE_SOURCE == "push"
+ when: never
+ - if: '$CI_PIPELINE_SOURCE == "merge_request_event"'
+ - if: '$CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS'
+ when: never
+ - if: '$CI_COMMIT_BRANCH'
+
stages:
- license
- build_and_test
@@ -15,7 +25,6 @@ stages:
- echo $GNSSUTILS_BRANCH
- echo $LASERSCANUTILS_BRANCH
- echo $CI_COMMIT_BRANCH
- - echo $WOLF_ROS_NODE_BRANCH
- echo $WOLF_ROS_IMU_BRANCH
- echo $WOLF_ROS_GNSS_BRANCH
- echo $WOLF_ROS_LASER_BRANCH
@@ -90,8 +99,7 @@ stages:
- if [ -d wolf ]; then
- echo "directory wolf exists"
- cd wolf
- - git checkout devel
- - git pull
+ - git fetch --all
- git checkout $WOLF_CORE_BRANCH
- git pull
- else
@@ -198,7 +206,7 @@ deploy_vision:
WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
WOLF_ROS_NODE_BRANCH: $CI_COMMIT_BRANCH
trigger:
- project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_apriltag
+ project: mobile_robotics/wolf_projects/wolf_ros/wolf_ros_vision
branch: $WOLF_ROS_VISION_BRANCH
deploy_apriltag:
diff --git a/CMakeLists.txt b/CMakeLists.txt
index dfb477604a3919687abde4cd76fd6618c185bacd..5331b32813d9ad1be6b1431bbc16ed83e83369c1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,7 +4,7 @@ project(wolf_ros_node)
## Compile as C++14
add_compile_options(-std=c++14)
# -fPIC and -rdynamic ensure unique singleton instance across shared libraries (for factories) see: https://stackoverflow.com/a/8626922
-SET(CMAKE_CXX_FLAGS "-fPIC -rdynamic")
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -rdynamic")
find_package(catkin REQUIRED COMPONENTS
roslib
@@ -15,11 +15,25 @@ find_package(catkin REQUIRED COMPONENTS
tf
tf_conversions
tf2_ros
+ message_generation
)
## System dependencies are found with CMake's conventions
find_package(wolfcore REQUIRED)
+## Generate messages in the 'msg' folder
+add_message_files(
+ FILES
+ LandmarkDetection.msg
+ LandmarkDetectionArray.msg
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+ DEPENDENCIES
+ geometry_msgs
+ std_msgs
+)
###################################
## catkin specific configuration ##
@@ -27,6 +41,7 @@ find_package(wolfcore REQUIRED)
# produces the XXXConfig.cmake file among other things
catkin_package(
INCLUDE_DIRS include
+ CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
)
###########
@@ -43,16 +58,21 @@ include_directories(
add_executable(${PROJECT_NAME} src/node.cpp)
add_library(subscriber_${PROJECT_NAME}
- src/subscriber_diffdrive.cpp
- src/subscriber_odom2d.cpp)
+ src/subscriber_diffdrive.cpp
+ src/subscriber_landmarks.cpp
+ src/subscriber_odom2d.cpp
+ src/subscriber_pose.cpp)
+
+# messages generated are a dependency of subscriber_landmarks
+add_dependencies(subscriber_${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_library(publisher_${PROJECT_NAME}
src/publisher_graph.cpp
src/publisher_pose.cpp
- src/publisher_trajectory.cpp
src/publisher_state_block.cpp
- src/publisher_tf.cpp)
-
+ src/publisher_tf.cpp
+ src/publisher_trajectory.cpp)
+
## Specify libraries to link a library or executable target against
target_link_libraries(subscriber_${PROJECT_NAME}
${catkin_LIBRARIES}
diff --git a/include/factory_publisher.h b/include/factory_publisher.h
index 1911c43b42d8a01516c615223b8f42a237178a0e..b64e40f72c2f1260b1724f8ce2d7ac20ef90196f 100644
--- a/include/factory_publisher.h
+++ b/include/factory_publisher.h
@@ -25,6 +25,8 @@
// wolf
#include <core/common/factory.h>
#include <core/utils/params_server.h>
+
+// ros
#include <ros/ros.h>
namespace wolf
diff --git a/include/factory_subscriber.h b/include/factory_subscriber.h
index dc23500bc1842bcb79acec461f72603dbba1ded8..6d2eaf9f3cbbaf38514b880dd6efceb41835b96a 100644
--- a/include/factory_subscriber.h
+++ b/include/factory_subscriber.h
@@ -25,10 +25,9 @@
// wolf
#include <core/common/factory.h>
#include <core/utils/params_server.h>
-#include <ros/ros.h>
-// #include "wolf_ros_subscriber.h"
-// std
+// ros
+#include <ros/ros.h>
namespace wolf
diff --git a/include/node.h b/include/node.h
index 8324843dbbc865276bb527563164660e32e5b19a..d4c2dde1ee2d044bc501f74bf1c09d4f21d81c7f 100644
--- a/include/node.h
+++ b/include/node.h
@@ -19,29 +19,33 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+
+#ifndef NODE_H
+#define NODE_H
+
+/**************************
+ * WOLF ROS includes *
+ **************************/
+#include "subscriber.h"
+#include "publisher.h"
+
/**************************
* WOLF includes *
**************************/
-#include <core/common/node_base.h>
#include <core/common/wolf.h>
-#include <core/capture/capture_odom_2d.h>
-#include <core/ceres_wrapper/solver_ceres.h>
-#include <core/sensor/sensor_odom_2d.h>
-#include <core/processor/processor_odom_2d.h>
+#include <core/solver/solver_manager.h>
#include <core/problem/problem.h>
#include <core/utils/loader.h>
-#include <core/yaml/parser_yaml.h>
-#include <core/solver/factory_solver.h>
/**************************
- * CERES includes *
+ * ROS includes *
**************************/
#include <ros/ros.h>
#include <ros/package.h>
#include <nav_msgs/Odometry.h>
-#include "tf/LinearMath/Transform.h"
-#include "tf/transform_datatypes.h"
+#include <tf/LinearMath/Transform.h>
+#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
@@ -57,8 +61,6 @@
#include <fstream>
#include <string>
-#include "subscriber.h"
-#include "publisher.h"
using namespace wolf;
@@ -78,7 +80,7 @@ class WolfRosNode
protected:
- //std::vector<std::shared_ptr<Loader>> loaders_;
+ std::vector<std::shared_ptr<Loader>> loaders_;
// solver
SolverManagerPtr solver_;
@@ -111,3 +113,5 @@ class WolfRosNode
void createProfilingFile();
};
+
+#endif // NODE_H
\ No newline at end of file
diff --git a/include/publisher.h b/include/publisher.h
index e86ad782577d46932d9ff9f61d21ab6c78f7be4a..e595811d746744319dbe7e42576c47a77c7efa29 100644
--- a/include/publisher.h
+++ b/include/publisher.h
@@ -21,6 +21,9 @@
//--------LICENSE_END--------
#ifndef WOLF_PUBLISHER_H
#define WOLF_PUBLISHER_H
+
+#include "factory_publisher.h"
+
/**************************
* ROS includes *
**************************/
@@ -29,9 +32,8 @@
/**************************
* WOLF includes *
**************************/
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "factory_publisher.h"
+#include <core/common/wolf.h>
+#include <core/problem/problem.h>
namespace wolf
{
diff --git a/include/publisher_graph.h b/include/publisher_graph.h
index 22bdd40aa09890400c3dcf71640b1f4ae573ce42..4cdf019cef9e1708f8b0216b58bdd1cb09c0443e 100644
--- a/include/publisher_graph.h
+++ b/include/publisher_graph.h
@@ -22,11 +22,12 @@
#ifndef PUBLISHER_GRAPH_H
#define PUBLISHER_GRAPH_H
+#include "publisher.h"
+
/**************************
* WOLF includes *
**************************/
-#include "core/problem/problem.h"
-#include "publisher.h"
+#include <core/problem/problem.h>
/**************************
* ROS includes *
diff --git a/include/publisher_pose.h b/include/publisher_pose.h
index 1e7428f959bc7fe78654e9266345e631dc658299..59a0ef334366dd0cb1fa6883550f0a9f2f50f904 100644
--- a/include/publisher_pose.h
+++ b/include/publisher_pose.h
@@ -22,12 +22,14 @@
#ifndef PUBLISHER_POSE_H
#define PUBLISHER_POSE_H
+
+#include "publisher.h"
+
/**************************
* WOLF includes *
**************************/
-#include "core/problem/problem.h"
+#include <core/problem/problem.h>
-#include "publisher.h"
/**************************
* ROS includes *
diff --git a/include/publisher_state_block.h b/include/publisher_state_block.h
index 78bd104683cecc9a68bbbdc08bb8871d97d83a15..c0d229cf8539f1bf52e452848501e4f0dafe1999 100644
--- a/include/publisher_state_block.h
+++ b/include/publisher_state_block.h
@@ -22,12 +22,12 @@
#ifndef PUBLISHER_STATE_BLOCK_H
#define PUBLISHER_STATE_BLOCK_H
+#include "publisher.h"
+
/**************************
* WOLF includes *
**************************/
-#include "core/problem/problem.h"
-
-#include "publisher.h"
+#include <core/problem/problem.h>
/**************************
* ROS includes *
diff --git a/include/publisher_tf.h b/include/publisher_tf.h
index 333676b43bf7c1a0da7317722431685d4e49fed8..fe545e53b558df0293150e1ed1671d37373f2115 100644
--- a/include/publisher_tf.h
+++ b/include/publisher_tf.h
@@ -22,12 +22,12 @@
#ifndef PUBLISHER_TF_H
#define PUBLISHER_TF_H
+#include "publisher.h"
+
/**************************
* WOLF includes *
**************************/
-#include "core/problem/problem.h"
-
-#include "publisher.h"
+#include <core/problem/problem.h>
/**************************
* ROS includes *
diff --git a/include/publisher_trajectory.h b/include/publisher_trajectory.h
index bd26b6cc366d3dfd139049f6ddf4cd9ac612f246..7ce9eaafa0ac0128f7aee3a441a5cc1362efce5f 100644
--- a/include/publisher_trajectory.h
+++ b/include/publisher_trajectory.h
@@ -28,12 +28,13 @@
#ifndef PUBLISHER_TRAJECTORY_H
#define PUBLISHER_TRAJECTORY_H
+#include "publisher.h"
+
/**************************
* WOLF includes *
**************************/
-#include "core/problem/problem.h"
+#include <core/problem/problem.h>
-#include "publisher.h"
/**************************
* ROS includes *
diff --git a/include/subscriber.h b/include/subscriber.h
index 0db5b5cad5158ef5aaa4e8371e03e2be0cb21d5a..5de7b2bb8ab20a022b4cbe921673ca3723a8b462 100644
--- a/include/subscriber.h
+++ b/include/subscriber.h
@@ -22,13 +22,14 @@
#ifndef WOLF_SUBSCRIBER_H_
#define WOLF_SUBSCRIBER_H_
+#include "factory_subscriber.h"
+
/**************************
* WOLF includes *
**************************/
-#include "core/common/wolf.h"
+#include <core/common/wolf.h>
#include <core/utils/params_server.h>
#include <core/sensor/sensor_base.h>
-#include "factory_subscriber.h"
/**************************
* ROS includes *
diff --git a/include/subscriber_diffdrive.h b/include/subscriber_diffdrive.h
index 3638e5b372884b07d3398d78fb0d4eb3bf8f172a..48f64e2c91bf6ca2b4cef8fca79b2950f26bcdde 100644
--- a/include/subscriber_diffdrive.h
+++ b/include/subscriber_diffdrive.h
@@ -19,27 +19,30 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+
+#ifndef WOLF_SUBSCRIBER_DIFFDRIVE_H_
+#define WOLF_SUBSCRIBER_DIFFDRIVE_H_
+
+
+#include "subscriber.h"
/**************************
* WOLF includes *
**************************/
#include <core/common/wolf.h>
-#include "subscriber.h"
/**************************
* ROS includes *
**************************/
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
-#include "sensor_msgs/JointState.h"
+#include <sensor_msgs/JointState.h>
namespace wolf
{
class SubscriberDiffdrive : public Subscriber
{
protected:
- ros::Time last_odom_stamp_;
Eigen::Vector2d last_angles_;
- int last_odom_seq_;
int last_kf = -1;
double ticks_cov_factor_;
@@ -56,3 +59,5 @@ class SubscriberDiffdrive : public Subscriber
};
} // namespace wolf
+
+#endif // WOLF_SUBSCRIBER_DIFFDRIVE_H_
\ No newline at end of file
diff --git a/include/subscriber_landmarks.h b/include/subscriber_landmarks.h
new file mode 100644
index 0000000000000000000000000000000000000000..f389542a57bfcc607692c7c772e56bf69bf89d93
--- /dev/null
+++ b/include/subscriber_landmarks.h
@@ -0,0 +1,60 @@
+//--------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--------
+#pragma once
+
+#include "subscriber.h"
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include <core/common/wolf.h>
+#include <core/utils/params_server.h>
+
+/**************************
+ * ROS includes *
+ **************************/
+#include <ros/ros.h>
+#include "wolf_ros_node/LandmarkDetectionArray.h"
+
+
+namespace wolf
+{
+class SubscriberLandmarks : public Subscriber
+{
+ protected:
+ SizeEigen dim;
+ bool inverse_detections_;
+ Eigen::Vector3d sensor_p_;
+ Eigen::Quaterniond sensor_q_;
+
+ public:
+
+ SubscriberLandmarks(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const SensorBasePtr _sensor_ptr);
+ WOLF_SUBSCRIBER_CREATE(SubscriberLandmarks);
+
+ virtual void initialize(ros::NodeHandle& nh, const std::string& topic);
+
+ void callback(const wolf_ros_node::LandmarkDetectionArray::ConstPtr& msg);
+};
+} // namespace wolf
\ No newline at end of file
diff --git a/include/subscriber_odom2d.h b/include/subscriber_odom2d.h
index db786e723b76f644efa934c8eeda30fcd7fcdb45..16ea0f0bc779e1d63bc384096b806fdbacc280f4 100644
--- a/include/subscriber_odom2d.h
+++ b/include/subscriber_odom2d.h
@@ -19,11 +19,19 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+
+#ifndef WOLF_SUBSCRIBER_ODOM2D_H_
+#define WOLF_SUBSCRIBER_ODOM2D_H_
+
+
+#include "subscriber.h"
+
/**************************
* WOLF includes *
**************************/
#include <core/common/wolf.h>
#include <core/utils/params_server.h>
+#include <core/sensor/sensor_odom_2d.h>
/**************************
* ROS includes *
@@ -31,7 +39,6 @@
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
-#include "subscriber.h"
namespace wolf
{
@@ -54,3 +61,5 @@ class SubscriberOdom2d : public Subscriber
};
} // namespace wolf
+
+#endif
\ No newline at end of file
diff --git a/include/subscriber_pose.h b/include/subscriber_pose.h
new file mode 100644
index 0000000000000000000000000000000000000000..27be7e4e349e2ce59fedd1a074c1a01dd34341b3
--- /dev/null
+++ b/include/subscriber_pose.h
@@ -0,0 +1,65 @@
+//--------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 WOLF_SUBSCRIBER_POSE_H_
+#define WOLF_SUBSCRIBER_POSE_H_
+
+#include "subscriber.h"
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include <core/common/wolf.h>
+#include <core/utils/params_server.h>
+#include <core/capture/capture_pose.h>
+#include <core/sensor/sensor_pose.h>
+#include <core/processor/processor_pose.h>
+
+/**************************
+ * ROS includes *
+ **************************/
+#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
+
+
+namespace wolf
+{
+class SubscriberPose : public Subscriber
+{
+ protected:
+ ros::Time last_pose_stamp_;
+ SensorPosePtr sensor_pose_;
+
+ public:
+
+ SubscriberPose(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const SensorBasePtr _sensor_ptr);
+ WOLF_SUBSCRIBER_CREATE(SubscriberPose);
+
+ virtual void initialize(ros::NodeHandle& nh, const std::string& topic);
+
+ void callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
+};
+
+} // namespace wolf
+
+#endif
\ No newline at end of file
diff --git a/msg/LandmarkDetection.msg b/msg/LandmarkDetection.msg
new file mode 100644
index 0000000000000000000000000000000000000000..640da05d6f75f583471ff5178b954845c7879d55
--- /dev/null
+++ b/msg/LandmarkDetection.msg
@@ -0,0 +1,3 @@
+geometry_msgs/PoseWithCovariance pose
+int32 id
+float32 quality
\ No newline at end of file
diff --git a/msg/LandmarkDetectionArray.msg b/msg/LandmarkDetectionArray.msg
new file mode 100644
index 0000000000000000000000000000000000000000..d59391da0cd9b0d219bb20cfd688a4d1faa9dd95
--- /dev/null
+++ b/msg/LandmarkDetectionArray.msg
@@ -0,0 +1,2 @@
+Header header
+LandmarkDetection[] detections
\ No newline at end of file
diff --git a/package.xml b/package.xml
index de43c538101cdb9822e207f24ef42eb7766058f3..93cfc209a2c38f60124d5aecb5b39520324262bd 100644
--- a/package.xml
+++ b/package.xml
@@ -49,29 +49,19 @@
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>sensor_msgs</build_depend>
- <build_depend>nav_msgs</build_depend>
- <build_depend>std_msgs</build_depend>
- <build_depend>tf</build_depend>
- <build_depend>tf2_ros</build_depend>
- <!-- <build_depend>roslib</build_depend> -->
- <build_export_depend>roscpp</build_export_depend>
- <build_export_depend>sensor_msgs</build_export_depend>
- <build_export_depend>nav_msgs</build_export_depend>
- <build_export_depend>std_msgs</build_export_depend>
- <build_export_depend>tf</build_export_depend>
- <build_export_depend>tf2_ros</build_export_depend>
- <exec_depend>roscpp</exec_depend>
- <exec_depend>sensor_msgs</exec_depend>
- <exec_depend>std_msgs</exec_depend>
- <exec_depend>tf</exec_depend>
- <exec_depend>tf2_ros</exec_depend>
- <!-- <exec_depend>roslib</exec_depend> -->
+ <depend>roscpp</depend>
+ <depend>std_msgs</depend>
+ <depend>geometry_msgs</depend>
+ <depend>sensor_msgs</depend>
+ <depend>nav_msgs</depend>
+ <depend>tf</depend>
+ <depend>tf2_ros</depend>
+ <build_depend>message_generation</build_depend>
+ <build_export_depend>message_runtime</build_export_depend>
+ <exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
-
</export>
</package>
diff --git a/src/node.cpp b/src/node.cpp
index 910cfdacd7ce47c71426bc8553d4dbf32cb9feea..7e102f812439d204fa69dc6e65142ca57ebcd263 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -19,14 +19,23 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include <chrono>
-#include "node.h"
-#include "ros/time.h"
-#include "core/solver/factory_solver.h"
-#include "tf/transform_datatypes.h"
+
+// wolf ros
#include "factory_subscriber.h"
#include "factory_publisher.h"
+// wolf
+#include <core/solver/factory_solver.h>
+#include <core/yaml/parser_yaml.h>
+
+// ros
+#include <node.h>
+#include <ros/time.h>
+#include <tf/transform_datatypes.h>
+
+// std
+#include <chrono>
+
WolfRosNode::WolfRosNode()
: nh_(ros::this_node::getName())
, last_print_(ros::Time(0))
@@ -58,6 +67,16 @@ WolfRosNode::WolfRosNode()
ROS_INFO("Creating problem...");
problem_ptr_ = Problem::autoSetup(server);
+ // // HACK -> SENSOR PRIORS
+ // auto imu = problem_ptr_->findSensor("IMU");
+ // double std_acc = server.getParam<double>("sensor/IMU/ab_initial_stdev");
+ // double std_gyro = server.getParam<double>("sensor/IMU/wb_initial_stdev");
+ // Array<double,6,1> std_bias;
+ // std_bias << std_acc, std_acc, std_acc, std_gyro, std_gyro, std_gyro;
+ // imu->addPriorParameter('I', // bias
+ // Vector6d::Zero(), // mean
+ // std_bias.square().matrix().asDiagonal()); // cov
+
// SOLVER
ROS_INFO("Creating solver...");
solver_ = FactorySolver::create("SolverCeres", problem_ptr_, server);
@@ -85,7 +104,7 @@ WolfRosNode::WolfRosNode()
WOLF_TRACE("Loading publisher " + type + " via " + lib_publisher);
auto l = std::make_shared<LoaderRaw>(lib_publisher);
l->load();
- //loaders_.push_back(l);
+ loaders_.push_back(l);
WOLF_INFO("Pub: ", type, " name: ", name);
publishers_.push_back(FactoryPublisher::create(type,
@@ -109,13 +128,17 @@ WolfRosNode::WolfRosNode()
WOLF_TRACE("Loading subscriber " + type + " via " + lib_subscriber);
auto l = std::make_shared<LoaderRaw>(lib_subscriber);
l->load();
- //loaders_.push_back(l);
+ loaders_.push_back(l);
+
+ auto sensor_ptr = problem_ptr_->findSensor(sensor);
+ if (not sensor_ptr)
+ throw std::runtime_error("Sensor " + sensor + " was not found!");
WOLF_TRACE("From sensor {" + sensor + "} subscribing {" + type + "} to {" + topic + "} topic");
subscribers_.push_back(FactorySubscriber::create(type,
name,
server,
- problem_ptr_->findSensor(sensor),
+ sensor_ptr,
nh_));
}
diff --git a/src/publisher_graph.cpp b/src/publisher_graph.cpp
index 562634885f86c85246bfd60f978a5154ff43c326..33e6aa24749ad9b7e6c6c63151681cd7abdad764 100644
--- a/src/publisher_graph.cpp
+++ b/src/publisher_graph.cpp
@@ -20,8 +20,13 @@
//
//--------LICENSE_END--------
#include "publisher_graph.h"
+
+// wolf
+#include <core/processor/processor_motion.h>
+
+// ros
#include <tf/transform_datatypes.h>
-#include "core/processor/processor_motion.h"
+
namespace wolf
{
@@ -316,6 +321,14 @@ void PublisherGraph::publishFactors()
// Iterate over the list of factors
for (auto fac : fac_list)
{
+ // reset factor_marker contents to minimum content
+ factor_marker = factor_marker_;
+
+ // check factor is valid (it is not being removed)
+ // (this is redundant with getFactorList() checks but just in case)
+ if (fac->isRemoving())
+ continue;
+
// Try to fill marker
if (not fillFactorMarker(fac, factor_marker, factor_text_marker))
continue;
@@ -410,31 +423,57 @@ bool PublisherGraph::fillLandmarkMarkers(LandmarkBaseConstPtr lmk,
return false;
// SHAPE ------------------------------------------------------
+ // Pose
+ // 2d: ARROW
+ // 3d: FRAME
// Position
// 2d: CYLINDER
// 3d: SPHERE
- // Pose -> ARROW
- if (lmk->getO() != nullptr)
+ if (lmk->getO() and lmk->getO())
{
- landmark_marker_.type = visualization_msgs::Marker::ARROW;
- lmk_marker.scale.x = viz_scale_*landmark_length_;
- lmk_marker.scale.y = viz_scale_*landmark_width_;
- lmk_marker.scale.z = viz_scale_*landmark_width_;
+ if (lmk->getP()->getSize() == 2)
+ {
+ landmark_marker_.type = visualization_msgs::Marker::ARROW;
+ lmk_marker.scale.x = viz_scale_*landmark_length_;
+ lmk_marker.scale.y = viz_scale_*landmark_width_;
+ lmk_marker.scale.z = viz_scale_*landmark_width_;
+ }
+ else if (lmk->getP()->getSize() == 3)
+ {
+ landmark_marker_.type = visualization_msgs::Marker::LINE_LIST;
+ lmk_marker.scale.x = viz_scale_*landmark_width_;
+
+ lmk_marker.points = frame_marker_.points;
+ lmk_marker.points[1].x = viz_scale_*landmark_length_;
+ lmk_marker.points[3].y = viz_scale_*landmark_length_;
+ lmk_marker.points[5].z = viz_scale_*landmark_length_;
+
+ lmk_marker.colors = frame_marker_.colors;
+ }
+ else
+ return false;
}
- else if (lmk->getP()->getSize() == 2)
+ else if (lmk->getP())
{
- landmark_marker_.type = visualization_msgs::Marker::CYLINDER;
- lmk_marker.scale.x = viz_scale_*landmark_width_;
- lmk_marker.scale.y = viz_scale_*landmark_width_;
- lmk_marker.scale.z = viz_scale_*landmark_length_;
+ if (lmk->getP()->getSize() == 2)
+ {
+ landmark_marker_.type = visualization_msgs::Marker::CYLINDER;
+ lmk_marker.scale.x = viz_scale_*landmark_width_;
+ lmk_marker.scale.y = viz_scale_*landmark_width_;
+ lmk_marker.scale.z = viz_scale_*landmark_length_;
+ }
+ else if (lmk->getP()->getSize() == 3)
+ {
+ landmark_marker_.type = visualization_msgs::Marker::SPHERE;
+ lmk_marker.scale.x = viz_scale_*landmark_width_;
+ lmk_marker.scale.y = viz_scale_*landmark_width_;
+ lmk_marker.scale.z = viz_scale_*landmark_width_;
+ }
+ else
+ return false;
}
else
- {
- landmark_marker_.type = visualization_msgs::Marker::SPHERE;
- lmk_marker.scale.x = viz_scale_*landmark_width_;
- lmk_marker.scale.y = viz_scale_*landmark_width_;
- lmk_marker.scale.z = viz_scale_*landmark_width_;
- }
+ return false;
// COLOR ------------------------------------------------------
if (lmk->getHits() > landmark_max_hits_)
@@ -506,28 +545,41 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
geometry_msgs::Point point1, point2;
// point1 -> frame ------------------------------------------------------
- if (not fac->getCapture() or
- not fac->getCapture()->getFrame() or
- not fac->getCapture()->getFrame()->getP())
- return false;
-
- point1.x = fac->getCapture()->getFrame()->getP()->getState()(0);
- point1.y = fac->getCapture()->getFrame()->getP()->getState()(1);
+ auto capture = fac->getCapture();
+ if (not capture or capture->isRemoving()) return false;
+ auto frame = capture->getFrame();
+ if (not frame or frame->isRemoving()) return false;
+ auto position = frame->getP();
+ if (not position) return false;
+
+ point1.x = position->getState()(0);
+ point1.y = position->getState()(1);
if (fac->getProblem()->getDim() == 3)
- point1.z = fac->getCapture()->getFrame()->getP()->getState()(2);
+ point1.z = position->getState()(2);
else
point1.z = 0;
+
+
// point2 -> other ------------------------------------------------------
+ auto frame_other = fac->getFrameOther();
+ auto capture_other = fac->getCaptureOther();
+ auto feature_other = fac->getFeatureOther();
+ auto landmark_other = fac->getLandmarkOther();
+
// FRAME
- if (fac->getFrameOther() != nullptr)
+ if (frame_other != nullptr)
{
+ if (frame_other->isRemoving()) return false;
+ auto position_other = frame_other->getP();
+ if (not position_other) return false;
+
// special case: Motion from ProcessorMotion
auto proc_motion = std::dynamic_pointer_cast<const ProcessorMotion>(fac->getProcessor());
- if (proc_motion and fac->getCaptureOther())
+ if (proc_motion and capture_other and not capture_other->isRemoving())
{
// Get state of other
- auto x_other = fac->getFrameOther()->getState(proc_motion->getStateStructure());
+ auto x_other = frame_other->getState(proc_motion->getStateStructure());
// Get most recent motion
auto cap_own = std::static_pointer_cast<const CaptureMotion>(fac->getFeature()->getCapture());
@@ -543,7 +595,7 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
if ( proc_motion->hasCalibration())
{
// Get current calibration -- from other capture
- auto calib = proc_motion->getCalibration(fac->getCaptureOther());
+ auto calib = proc_motion->getCalibration(capture_other);
// get Jacobian of delta wrt calibration
const auto& J_delta_calib = motion.jacobian_calib_;
@@ -555,20 +607,20 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
auto delta_corrected = proc_motion->correctDelta(delta_preint, delta_step);
// compute current state // this is [+]
- proc_motion->statePlusDelta(x_other, delta_corrected, cap_own->getTimeStamp() - fac->getCaptureOther()->getTimeStamp(), state_integrated);
+ proc_motion->statePlusDelta(x_other, delta_corrected, cap_own->getTimeStamp() - capture_other->getTimeStamp(), state_integrated);
}
else
{
- proc_motion->statePlusDelta(x_other, delta_preint, cap_own->getTimeStamp() - fac->getCaptureOther()->getTimeStamp(), state_integrated);
+ proc_motion->statePlusDelta(x_other, delta_preint, cap_own->getTimeStamp() - capture_other->getTimeStamp(), state_integrated);
}
// FILL POINTS
// 1=origin (other)
- point1.x = fac->getFrameOther()->getP()->getState()(0);
- point1.y = fac->getFrameOther()->getP()->getState()(1);
+ point1.x = position_other->getState()(0);
+ point1.y = position_other->getState()(1);
if (fac->getProblem()->getDim() == 3)
- point1.z = fac->getFrameOther()->getP()->getState()(2);
+ point1.z = position_other->getState()(2);
else
point1.z = 0;
// 2=own
@@ -582,59 +634,53 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
// Normal frame-frame factor
else
{
- if (fac->getFrameOther()->isRemoving() or
- not fac->getFrameOther() or
- not fac->getFrameOther()->getP())
- return false;
-
- point2.x = fac->getFrameOther()->getP()->getState()(0);
- point2.y = fac->getFrameOther()->getP()->getState()(1);
+ point2.x = position_other->getState()(0);
+ point2.y = position_other->getState()(1);
if (fac->getProblem()->getDim() == 3)
- point2.z = fac->getFrameOther()->getP()->getState()(2);
+ point2.z = position_other->getState()(2);
else
point2.z = 0;
}
}
// CAPTURE
- else if (fac->getCaptureOther() != nullptr)
+ else if (capture_other != nullptr and not capture_other->isRemoving())
{
- if (fac->getCaptureOther()->isRemoving() or
- not fac->getCaptureOther()->getFrame() or
- not fac->getCaptureOther()->getFrame()->getP())
- return false;
+ auto frame_capture_other = capture_other->getFrame();
+ if (not frame_capture_other or frame_capture_other->isRemoving()) return false;
+ auto position = frame_capture_other->getP();
+ if (not position) return false;
- point2.x = fac->getCaptureOther()->getFrame()->getP()->getState()(0);
- point2.y = fac->getCaptureOther()->getFrame()->getP()->getState()(1);
+ point2.x = position->getState()(0);
+ point2.y = position->getState()(1);
if (fac->getProblem()->getDim() == 3)
- point2.z = fac->getCaptureOther()->getFrame()->getP()->getState()(2);
+ point2.z = position->getState()(2);
else
point2.z = 0;
}
// FEATURE
- else if (fac->getFeatureOther() != nullptr)
+ else if (feature_other != nullptr and not feature_other->isRemoving())
{
- if (fac->getFeatureOther()->isRemoving() or
- not fac->getFeatureOther()->getCapture() or
- not fac->getFeatureOther()->getCapture()->getFrame() or
- not fac->getFeatureOther()->getCapture()->getFrame()->getP())
- return false;
-
- point2.x = fac->getFeatureOther()->getCapture()->getFrame()->getP()->getState()(0);
- point2.y = fac->getFeatureOther()->getCapture()->getFrame()->getP()->getState()(1);
+ auto capture_feature_other = feature_other->getCapture();
+ if (not capture_feature_other or capture_feature_other->isRemoving()) return false;
+ auto frame_feature_other = capture_feature_other->getFrame();
+ if (not frame_feature_other or frame_feature_other->isRemoving()) return false;
+ auto position_other = frame_feature_other->getP();
+ if (not position_other) return false;
+
+ point2.x = position_other->getState()(0);
+ point2.y = position_other->getState()(1);
if (fac->getProblem()->getDim() == 3)
- point2.z = fac->getFeatureOther()->getCapture()->getFrame()->getP()->getState()(2);
+ point2.z = position_other->getState()(2);
else
point2.z = 0;
}
// LANDMARK
- else if (fac->getLandmarkOther() != nullptr)
+ else if (landmark_other != nullptr and not landmark_other->isRemoving())
{
- if (fac->getLandmarkOther()->isRemoving() or
- not fac->getLandmarkOther()->getP())
- return false;
+ auto position_other = landmark_other->getP();
+ if (not position_other) return false;
- Eigen::VectorXd lmk_pos;
- lmk_pos = fac->getLandmarkOther()->getP()->getState();
+ Eigen::VectorXd lmk_pos = position_other->getState();
point2.x = lmk_pos(0);
point2.y = lmk_pos(1);
if (fac->getProblem()->getDim() == 3)
@@ -659,6 +705,7 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
}
// store points ------------------------------------------------------
+ fac_marker.points.clear();
fac_marker.points.push_back(point1);
fac_marker.points.push_back(point2);
@@ -687,16 +734,17 @@ bool PublisherGraph::fillFactorMarker(FactorBaseConstPtr fac,
if (fac->getStatus() == FAC_INACTIVE)
color.a *= 0.5;
+ fac_marker.colors.clear();
fac_marker.colors.push_back(color);
fac_marker.colors.push_back(color);// 2 times because of 2 points
- fac_marker.ns = std::string("factors_"+(fac->getProcessor() ? fac->getProcessor()->getName() : "unnamed_processor"));
+ fac_marker.ns = std::string("factors_"+ fac->getType());
// TEXT MARKER --------------------------------------------------------
fac_text_marker.text = std::to_string(fac->id());
fac_text_marker.pose.position.x = (point1.x + point2.x)/(double) 2;
fac_text_marker.pose.position.y = (point1.y + point2.y)/(double) 2;
fac_text_marker.pose.position.z = (point1.z + point2.z)/(double) 2;
- fac_text_marker.ns = std::string("factors_text_"+(fac->getProcessor() ? fac->getProcessor()->getName() : "unnamed_processor"));
+ fac_text_marker.ns = std::string("factors_text_"+ fac->getType());
return true;
}
@@ -795,7 +843,7 @@ std::string PublisherGraph::factorString(FactorBaseConstPtr fac) const
// ABSOLUTE (nothing
// Topology
- factor_string += "_T" + fac->getTopology();
+ factor_string += "_T-" + fac->getTopologyString();
// Processor
factor_string += "_P" + (fac->getProcessor() ? std::to_string(fac->getProcessor()->id()) : "NO");
diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp
index abcecb335300a170dfc443f26511f4b30f6dfbd1..65222db813e14cb72d63cd723209d094ea996497 100644
--- a/src/publisher_pose.cpp
+++ b/src/publisher_pose.cpp
@@ -19,14 +19,16 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+
#include "publisher_pose.h"
/**************************
* ROS includes *
**************************/
#include <ros/ros.h>
-#include "tf/transform_datatypes.h"
-#include "tf_conversions/tf_eigen.h"
+#include <tf/transform_datatypes.h>
+#include <tf_conversions/tf_eigen.h>
+
namespace wolf
{
diff --git a/src/publisher_state_block.cpp b/src/publisher_state_block.cpp
index 8300340954bef448333c990eb8ab2462ad4016bc..f2a698d122f63aebe3722904343487791e33490b 100644
--- a/src/publisher_state_block.cpp
+++ b/src/publisher_state_block.cpp
@@ -19,6 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+
#include "publisher_state_block.h"
namespace wolf
diff --git a/src/publisher_tf.cpp b/src/publisher_tf.cpp
index 2591644b7278b9a0c1dbf2e174c2834afd03a061..448d9bb6fc7a159e1c5c895f2825cc4370fc8a6a 100644
--- a/src/publisher_tf.cpp
+++ b/src/publisher_tf.cpp
@@ -19,14 +19,15 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+
#include "publisher_tf.h"
/**************************
* ROS includes *
**************************/
#include <ros/ros.h>
-#include "tf/transform_datatypes.h"
-#include "tf_conversions/tf_eigen.h"
+#include <tf/transform_datatypes.h>
+#include <tf_conversions/tf_eigen.h>
namespace wolf
{
diff --git a/src/subscriber_diffdrive.cpp b/src/subscriber_diffdrive.cpp
index da7a29504cadb1209c504a26892f1d443681d462..815e5f84942324e0760dbf85d1f2c90c7e0ad6bb 100644
--- a/src/subscriber_diffdrive.cpp
+++ b/src/subscriber_diffdrive.cpp
@@ -19,14 +19,16 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+
+#include "subscriber_diffdrive.h"
+
/**************************
* WOLF includes *
**************************/
#include <core/capture/capture_diff_drive.h>
#include <core/sensor/sensor_diff_drive.h>
+#include <core/math/rotations.h>
-#include "core/math/rotations.h"
-#include "subscriber_diffdrive.h"
namespace wolf
{
@@ -34,8 +36,6 @@ SubscriberDiffdrive::SubscriberDiffdrive(const std::string& _unique_name,
const ParamsServer& _server,
const SensorBasePtr _sensor_ptr)
: Subscriber(_unique_name, _server, _sensor_ptr)
-, last_odom_stamp_(ros::Time(0))
-, last_odom_seq_(-1)
{
last_angles_ = Eigen::Vector2d();
ticks_cov_factor_ = std::static_pointer_cast<SensorDiffDrive>(_sensor_ptr)->getParams()->ticks_cov_factor;
@@ -60,7 +60,7 @@ void SubscriberDiffdrive::callback(const sensor_msgs::JointState::ConstPtr& msg)
Eigen::MatrixXd cov = ticks_cov_factor_ * (angles_inc.cwiseAbs() + Eigen::Vector2d::Ones()).asDiagonal(); // TODO check this
- if (last_odom_seq_ != -1)
+ if (last_seq_ != -1)
{
CaptureDiffDrivePtr cptr = std::make_shared<CaptureDiffDrive>(
TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), sensor_ptr_, angles_inc, cov, nullptr);
@@ -80,12 +80,9 @@ void SubscriberDiffdrive::callback(const sensor_msgs::JointState::ConstPtr& msg)
last_kf = current_kf;
}
-
}
last_angles_ = msg_angles;
- last_odom_stamp_ = msg->header.stamp;
- last_odom_seq_ = msg->header.seq;
}
WOLF_REGISTER_SUBSCRIBER(SubscriberDiffdrive)
diff --git a/src/subscriber_landmarks.cpp b/src/subscriber_landmarks.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..503b9f1cd735c29a8394f4400642f38407418f80
--- /dev/null
+++ b/src/subscriber_landmarks.cpp
@@ -0,0 +1,129 @@
+//--------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 "subscriber_landmarks.h"
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include <core/capture/capture_landmarks_external.h>
+#include <core/math/covariance.h>
+#include <core/math/rotations.h>
+#include <core/math/SE3.h>
+
+/**************************
+ * ROS includes *
+ **************************/
+// #include <tf/transform_datatypes.h>
+
+using namespace Eigen;
+
+namespace wolf
+{
+SubscriberLandmarks::SubscriberLandmarks(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const SensorBasePtr _sensor_ptr)
+ : Subscriber(_unique_name, _server, _sensor_ptr)
+{
+ assert(_sensor_ptr);
+ dim = _sensor_ptr->getProblem()->getDim();
+ inverse_detections_ = _server.getParam<bool>(prefix_ + "/inverse_detections");
+ auto sensor_extrinsics = _server.getParam<VectorXd>(prefix_ + "/sensor_extrinsics");
+ sensor_p_ = sensor_extrinsics.head<3>();
+ sensor_q_ = Quaterniond(Vector4d(sensor_extrinsics.tail<4>()));
+}
+
+void SubscriberLandmarks::initialize(ros::NodeHandle& nh, const std::string& topic)
+{
+ sub_ = nh.subscribe(topic, 100, &SubscriberLandmarks::callback, this);
+}
+
+void SubscriberLandmarks::callback(const wolf_ros_node::LandmarkDetectionArray::ConstPtr& msg)
+{
+ ROS_INFO("SubscriberLandmarks::callback: %lu detections", msg->detections.size());
+
+ updateLastHeader(msg->header);
+
+ auto cap = std::make_shared<CaptureLandmarksExternal>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), sensor_ptr_);
+ // Extract detections from msg
+ for (auto i = 0; i < msg->detections.size(); i++)
+ {
+ // 3D measurement from msg
+ VectorXd meas(7);
+ meas << msg->detections.at(i).pose.pose.position.x,
+ msg->detections.at(i).pose.pose.position.y,
+ msg->detections.at(i).pose.pose.position.z,
+ msg->detections.at(i).pose.pose.orientation.x,
+ msg->detections.at(i).pose.pose.orientation.y,
+ msg->detections.at(i).pose.pose.orientation.z,
+ msg->detections.at(i).pose.pose.orientation.w;
+
+ meas.head<3>() = sensor_p_ + sensor_q_ * meas.head<3>();
+ meas.tail<4>() = (sensor_q_ * Quaterniond(Vector4d(meas.tail<4>()))).coeffs();
+
+ // PoseWithCovariance documentation:
+ // Row-major representation of the 6x6 covariance matrix.
+ // The orientation parameters use a fixed-axis representation.
+ // In order, the parameters are: (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
+ MatrixXd cov = Eigen::Map<const Eigen::Matrix6d>(msg->detections.at(i).pose.covariance.data());
+ auto R = q2R(meas.tail<4>());
+ cov.topLeftCorner<3,3>() = R * cov.topLeftCorner<3,3>() * R.transpose();
+ //TODO: rotate covariance for orientation
+
+ // inverse measurement
+ if (inverse_detections_)
+ {
+ meas = SE3::inverse(meas);
+ cov.topLeftCorner<3,3>() = R.transpose() * cov.topLeftCorner<3,3>() * R;
+ //TODO: rotate covariance for orientation
+ }
+
+ // 2D
+ if (dim == 2)
+ {
+ VectorXd meas2d = meas.head<3>();
+ meas2d(2) = getYaw(q2R(meas.tail<4>()));
+
+ MatrixXd cov2d(3,3);
+ cov2d << cov(0,0), cov(0,1), cov(0,5),
+ cov(1,0), cov(1,1), cov(1,5),
+ cov(5,0), cov(5,1), cov(5,5);
+
+ meas = meas2d;
+ cov = cov2d;
+ }
+
+ std::cout << "\tid " << msg->detections.at(i).id << ": quality: " << msg->detections.at(i).quality << ", meas: " << meas.transpose() << std::endl;
+
+ // fill capture
+ makePosDef(cov);
+ cap->addDetection(msg->detections.at(i).id, meas, cov, msg->detections.at(i).quality);
+ }
+
+ // process
+ sensor_ptr_->process(cap);
+
+ ROS_DEBUG("SubscriberLandmarks::callback: end");
+}
+
+WOLF_REGISTER_SUBSCRIBER(SubscriberLandmarks)
+} // namespace wolf
diff --git a/src/subscriber_odom2d.cpp b/src/subscriber_odom2d.cpp
index f46d0a5e2eb8623bd16845d86f3dca1fb90f051f..699f10d361a3b90923fe8cbcc52f9293978ec74a 100644
--- a/src/subscriber_odom2d.cpp
+++ b/src/subscriber_odom2d.cpp
@@ -19,6 +19,10 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+
+#include "subscriber.h"
+#include "subscriber_odom2d.h"
+
/**************************
* WOLF includes *
**************************/
@@ -39,9 +43,6 @@
#include <iomanip>
#include <queue>
-#include "subscriber.h"
-#include "subscriber_odom2d.h"
-
namespace wolf
{
SubscriberOdom2d::SubscriberOdom2d(const std::string& _unique_name,
diff --git a/src/subscriber_pose.cpp b/src/subscriber_pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..70fddeac734105fecf453db7884dad49717e55e8
--- /dev/null
+++ b/src/subscriber_pose.cpp
@@ -0,0 +1,79 @@
+//--------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 "subscriber_pose.h"
+
+/**************************
+ * WOLF includes *
+ **************************/
+
+/**************************
+ * ROS includes *
+ **************************/
+
+/**************************
+ * STD includes *
+ **************************/
+#include <iostream>
+#include <iomanip>
+#include <queue>
+
+
+namespace wolf
+{
+SubscriberPose::SubscriberPose(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const SensorBasePtr _sensor_ptr)
+ : Subscriber(_unique_name, _server, _sensor_ptr)
+ , last_pose_stamp_(ros::Time(0))
+ , sensor_pose_(std::static_pointer_cast<SensorPose>(_sensor_ptr))
+{
+ assert(std::dynamic_pointer_cast<SensorPose>(_sensor_ptr) != nullptr && "SubscriberPose: sensor provided is not of type SensorPose!");
+}
+
+void SubscriberPose::initialize(ros::NodeHandle& nh, const std::string& topic)
+{
+ sub_ = nh.subscribe(topic, 100, &SubscriberPose::callback, this);
+}
+
+void SubscriberPose::callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+ updateLastHeader(msg->header);
+
+ if (last_pose_stamp_ != ros::Time(0))
+ {
+ double dt = (msg->header.stamp - last_pose_stamp_).toSec();
+ Eigen::Vector7d data;
+ data << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w;
+ CapturePosePtr new_capture = std::make_shared<CapturePose>(
+ TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
+ sensor_ptr_,
+ data,
+ sensor_pose_->getNoiseCov());
+ sensor_ptr_->process(new_capture);
+ }
+ last_pose_stamp_ = msg->header.stamp;
+
+}
+
+WOLF_REGISTER_SUBSCRIBER(SubscriberPose)
+} // namespace wolf