Skip to content
Snippets Groups Projects
Commit 5ac1737a authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Merge branch 'devel' of...

Merge branch 'devel' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node into devel
parents c5349fef 4221cbd6
No related branches found
No related tags found
1 merge request!16Devel
...@@ -44,14 +44,15 @@ add_executable(${PROJECT_NAME} src/node.cpp) ...@@ -44,14 +44,15 @@ add_executable(${PROJECT_NAME} src/node.cpp)
add_library(subscriber_${PROJECT_NAME} add_library(subscriber_${PROJECT_NAME}
src/subscriber_diffdrive.cpp src/subscriber_diffdrive.cpp
src/subscriber_odom2d.cpp) src/subscriber_odom2d.cpp
src/subscriber_pose.cpp)
add_library(publisher_${PROJECT_NAME} add_library(publisher_${PROJECT_NAME}
src/publisher_graph.cpp src/publisher_graph.cpp
src/publisher_pose.cpp src/publisher_pose.cpp
src/publisher_trajectory.cpp
src/publisher_state_block.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 ## Specify libraries to link a library or executable target against
target_link_libraries(subscriber_${PROJECT_NAME} target_link_libraries(subscriber_${PROJECT_NAME}
......
...@@ -19,6 +19,10 @@ ...@@ -19,6 +19,10 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
//--------LICENSE_END-------- //--------LICENSE_END--------
#ifndef NODE_H
#define NODE_H
/************************** /**************************
* WOLF includes * * WOLF includes *
**************************/ **************************/
...@@ -111,3 +115,5 @@ class WolfRosNode ...@@ -111,3 +115,5 @@ class WolfRosNode
void createProfilingFile(); void createProfilingFile();
}; };
#endif // NODE_H
\ No newline at end of file
...@@ -19,6 +19,11 @@ ...@@ -19,6 +19,11 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
//--------LICENSE_END-------- //--------LICENSE_END--------
#ifndef WOLF_SUBSCRIBER_DIFFDRIVE_H_
#define WOLF_SUBSCRIBER_DIFFDRIVE_H_
/************************** /**************************
* WOLF includes * * WOLF includes *
**************************/ **************************/
...@@ -56,3 +61,5 @@ class SubscriberDiffdrive : public Subscriber ...@@ -56,3 +61,5 @@ class SubscriberDiffdrive : public Subscriber
}; };
} // namespace wolf } // namespace wolf
#endif // WOLF_SUBSCRIBER_DIFFDRIVE_H_
\ No newline at end of file
...@@ -19,6 +19,11 @@ ...@@ -19,6 +19,11 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
//--------LICENSE_END-------- //--------LICENSE_END--------
#ifndef WOLF_SUBSCRIBER_ODOM2D_H_
#define WOLF_SUBSCRIBER_ODOM2D_H_
/************************** /**************************
* WOLF includes * * WOLF includes *
**************************/ **************************/
...@@ -54,3 +59,5 @@ class SubscriberOdom2d : public Subscriber ...@@ -54,3 +59,5 @@ class SubscriberOdom2d : public Subscriber
}; };
} // namespace wolf } // namespace wolf
#endif
\ No newline at end of file
//--------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_
/**************************
* WOLF includes *
**************************/
#include "subscriber.h"
#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
//--------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--------
/**************************
* WOLF includes *
**************************/
#include "subscriber_pose.h"
/**************************
* 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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment