YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Navigation2D_nwc_yarp_iLocalization2D.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: LGPL-2.1-or-later
4 */
5
8#include <yarp/os/Log.h>
10#include <yarp/os/LogStream.h>
11#include <mutex>
12#include <cmath>
13
16using namespace yarp::dev;
17using namespace yarp::dev::Nav2D;
18using namespace yarp::os;
19using namespace yarp::sig;
20
21namespace {
22 YARP_LOG_COMPONENT(NAVIGATION2D_NWC, "yarp.device.navigation2D_nwc_yarp")
23}
24
25//------------------------------------------------------------------------------------------------------------------------------
26
28{
29 std::lock_guard <std::mutex> lg(m_mutex);
31}
32
34{
35 if (cov.rows() != 3 || cov.cols() != 3)
36 {
37 yCError(NAVIGATION2D_NWC) << "Covariance matrix is expected to have size (3,3)";
38 return false;
39 }
40
41 std::lock_guard <std::mutex> lg(m_mutex);
42 return m_loc_RPC.set_initial_pose2_RPC(loc, cov);
43}
44
46{
47 std::lock_guard <std::mutex> lg(m_mutex);
49 if (!ret.ret)
50 {
51 yCError(NAVIGATION2D_NWC, "Unable to get_current_position2_RPC");
52 return false;
53 }
54 loc = ret.loc;
55 cov = ret.cov;
56 return true;
57}
58
60{
61 std::lock_guard <std::mutex> lg(m_mutex);
63 if (!ret.ret)
64 {
65 yCError(NAVIGATION2D_NWC, "Unable to get_estimated_odometry_RPC");
66 return false;
67 }
68 odom = ret.odom;
69 return true;
70}
71
73{
74 std::lock_guard <std::mutex> lg(m_mutex);
76 if (!ret.ret)
77 {
78 yCError(NAVIGATION2D_NWC, "Unable to get_current_position1_RPC");
79 return false;
80 }
81 loc = ret.loc;
82 return true;
83}
84
86{
87 std::lock_guard <std::mutex> lg(m_mutex);
89 if (!ret.ret)
90 {
91 yCError(NAVIGATION2D_NWC, "Unable to get_localization_status_RPC");
92 return false;
93 }
95 return true;
96}
97
98bool Navigation2D_nwc_yarp::getEstimatedPoses(std::vector<Map2DLocation>& poses)
99{
100 std::lock_guard <std::mutex> lg(m_mutex);
102 if (!ret.ret)
103 {
104 yCError(NAVIGATION2D_NWC, "Unable to get_estimated_poses_RPC");
105 return false;
106 }
107 poses = ret.poses;
108 return true;
109}
110
112{
113 std::lock_guard <std::mutex> lg(m_mutex);
115}
116
118{
119 std::lock_guard <std::mutex> lg(m_mutex);
121}
bool ret
virtual return_get_estimated_poses get_estimated_poses_RPC()
virtual return_get_current_position1 get_current_position1_RPC()
virtual bool start_localization_service_RPC()
virtual return_get_estimated_odometry get_estimated_odometry_RPC()
virtual return_get_localization_status get_localization_status_RPC()
virtual bool set_initial_pose1_RPC(const yarp::dev::Nav2D::Map2DLocation &loc)
virtual return_get_current_position2 get_current_position2_RPC()
virtual bool set_initial_pose2_RPC(const yarp::dev::Nav2D::Map2DLocation &loc, const yarp::sig::Matrix &cov)
virtual bool stop_localization_service_RPC()
bool startLocalizationService() override
Starts the localization service.
ILocalization2DMsgs m_loc_RPC
bool setInitialPose(const yarp::dev::Nav2D::Map2DLocation &loc) override
Sets the initial pose for the localization algorithm which estimates the current position of the robo...
bool getCurrentPosition(yarp::dev::Nav2D::Map2DLocation &loc) override
Gets the current position of the robot w.r.t world reference frame.
bool getEstimatedOdometry(yarp::dev::OdometryData &odom) override
Gets the estimated odometry the robot, including its velocity expressed in the world and in the local...
bool getLocalizationStatus(yarp::dev::Nav2D::LocalizationStatusEnum &status) override
Gets the current status of the localization task.
bool getEstimatedPoses(std::vector< yarp::dev::Nav2D::Map2DLocation > &poses) override
Gets a set of pose estimates computed by the localization algorithm.
bool stopLocalizationService() override
Stops the localization service.
A mini-server for performing network communication in the background.
A class for a Matrix.
Definition Matrix.h:39
size_t cols() const
Return number of columns.
Definition Matrix.h:94
size_t rows() const
Return number of rows.
Definition Matrix.h:88
#define yCError(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.