Commit 2e84ac2d authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

new publisher

parent f1b41be6
......@@ -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
......
//--------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
//--------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_);
}
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment