YARP
Yet Another Robot Platform
FrameTransformClient.h
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 
6 #ifndef YARP_DEV_FRAMETRANSFORMCLIENT_H
7 #define YARP_DEV_FRAMETRANSFORMCLIENT_H
8 
9 
10 #include <yarp/os/BufferedPort.h>
11 #include <yarp/os/Network.h>
12 #include <yarp/os/PeriodicThread.h>
13 #include <yarp/os/Semaphore.h>
14 #include <yarp/os/Time.h>
15 
16 #include <yarp/sig/Vector.h>
17 
24 #include <yarp/dev/PolyDriver.h>
25 
27 #include <yarp/os/PeriodicThread.h>
28 #include <mutex>
31 
33 #include <mutex>
34 
35 #define DEFAULT_THREAD_PERIOD 20 //ms
36 const int MAX_PORTS = 5;
37 
67  public yarp::os::PortReader,
69 {
70 private:
71  enum class ConnectionType {DISCONNECTED = 0, DIRECT, INVERSE, UNDIRECT, IDENTITY};
72 
73  FrameTransformClient::ConnectionType priv_getConnectionType(const std::string &target_frame, const std::string &source_frame, std::string* commonAncestor);
74 
75  bool priv_canExplicitTransform(const std::string& target_frame_id, const std::string& source_frame_id) const;
76  bool priv_getChainedTransform(const std::string &target_frame_id, const std::string &source_frame_id, yarp::sig::Matrix &transform) const;
77 
78 protected:
79 
81  std::string m_local_name;
82  double m_period;
83  std::mutex m_rpc_mutex;
84 
85  //ports to broadcast stuff...
87  {
90  std::string transform_src;
91  std::string transform_dst;
92  };
93  std::vector<broadcast_port_t*> m_array_of_ports;
94 
95  //enum to generate tf diagrams
97  {
101  show_rpy = 3
102  };
104 
105  //new stuff
110 
111 
112 public:
115 
116 public:
117  // DeviceDriver methods
118  bool open(yarp::os::Searchable& config) override;
119  bool close() override;
120  bool read(yarp::os::ConnectionReader& connection) override;
121 
122  //IFrameTransform
123  bool allFramesAsString(std::string &all_frames) override;
124  bool canTransform(const std::string &target_frame, const std::string &source_frame) override;
125  bool clear() override;
126  bool frameExists(const std::string &frame_id) override;
127  bool getAllFrameIds(std::vector< std::string > &ids) override;
128  bool getParent(const std::string &frame_id, std::string &parent_frame_id) override;
129  bool getTransform(const std::string &target_frame_id, const std::string &source_frame_id, yarp::sig::Matrix &transform) override;
130  bool setTransform(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) override;
131  bool setTransformStatic(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) override;
132  bool deleteTransform(const std::string &target_frame_id, const std::string &source_frame_id) override;
133  bool transformPoint(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_point, yarp::sig::Vector &transformed_point) override;
134  bool transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose) override;
135  bool transformQuaternion(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::math::Quaternion &input_quaternion, yarp::math::Quaternion &transformed_quaternion) override;
136  bool waitForTransform(const std::string &target_frame_id, const std::string &source_frame_id, const double &timeout) override;
137 
138  //PeriodicThread
139  bool threadInit() override;
140  void threadRelease() override;
141  void run() override;
142 
143  //extra
144  bool priv_generate_view();
146 };
147 
148 #endif // YARP_DEV_FRAMETRANSFORMCLIENT_H
define control board standard interfaces
float t
const int MAX_PORTS
contains the definition of a Vector type
frameTransformClient: A client to manage FrameTransforms for a robot (For more information,...
yarp::dev::IFrameTransformStorageUtils * m_ift_util
bool waitForTransform(const std::string &target_frame_id, const std::string &source_frame_id, const double &timeout) override
Block until a transform from source_frame_id to target_frame_id is possible or it times out.
bool getAllFrameIds(std::vector< std::string > &ids) override
Gets a vector containing all the registered frames.
bool allFramesAsString(std::string &all_frames) override
Creates a debug string containing the list of all registered frames.
bool getParent(const std::string &frame_id, std::string &parent_frame_id) override
Get the parent of a frame.
void threadRelease() override
Release method.
show_transforms_in_diagram_t m_show_transforms_in_diagram
void run() override
Loop function.
bool deleteTransform(const std::string &target_frame_id, const std::string &source_frame_id) override
Deletes a transform between two frames.
yarp::dev::IFrameTransformStorageSet * m_ift_set
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool close() override
Close the DeviceDriver.
std::string priv_get_matrix_as_text(yarp::math::FrameTransform *t)
bool transformQuaternion(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::math::Quaternion &input_quaternion, yarp::math::Quaternion &transformed_quaternion) override
Transform a quaternion into the target frame.
bool transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose) override
Transform a Stamped Pose into the target frame.
bool clear() override
Removes all the registered transforms.
std::vector< broadcast_port_t * > m_array_of_ports
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
yarp::robotinterface::Robot m_robot
bool threadInit() override
Initialization method.
bool transformPoint(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_point, yarp::sig::Vector &transformed_point) override
Transform a point into the target frame.
bool setTransformStatic(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) override
Register a static transform between two frames.
bool frameExists(const std::string &frame_id) override
Check if a frame exists.
bool setTransform(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform) override
Register a transform between two frames.
yarp::os::Port m_rpc_InterfaceToUser
bool canTransform(const std::string &target_frame, const std::string &source_frame) override
Test if a transform exists.
yarp::dev::IFrameTransformStorageGet * m_ift_get
bool getTransform(const std::string &target_frame_id, const std::string &source_frame_id, yarp::sig::Matrix &transform) override
Get the transform between two frames.
Interface implemented by all device drivers.
Definition: DeviceDriver.h:35
Transform Interface.
An interface for reading from a network connection.
An abstraction for a periodic thread.
Interface implemented by all objects that can read themselves from the network, such as Bottle object...
Definition: PortReader.h:25
A mini-server for network communication.
Definition: Port.h:47
A base class for nested structures that can be searched.
Definition: Searchable.h:66
A class for a Matrix.
Definition: Matrix.h:43
enum FrameTransformClient::broadcast_port_t::format_t format