ros_robot_state_publisher_mt.hpp

Go to the documentation of this file.

#ifndef ROS_ROBOT_STATE_PUBLISHER_HPP
#define ROS_ROBOT_STATE_PUBLISHER_HPP

#include <deque>
#include <memory>
#include <mutex>
#include <thread>

#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/linear-algebra.h>

#include <realtime_tools/realtime_publisher.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf2_msgs/TFMessage.h>
#include <real_time_tools/spinner.hpp>
#include <real_time_tools/thread.hpp>

namespace dynamic_graph
{
typedef dynamicgraph::SignalTimeDependent<int, int> SignalOUT;
typedef dynamicgraph::SignalPtr<dynamicgraph::Vector, int> SignalIN;
typedef boost::function<void(int)> callback_t;

typedef realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> TfRtPublisher;

typedef realtime_tools::RealtimePublisher<sensor_msgs::JointState>
    JointStateRtPublisher;

struct RosRobotStatePublisherMtInternal
{
    std::shared_ptr<TfRtPublisher> base_tf_publisher_;
    std::shared_ptr<JointStateRtPublisher> joint_state_publisher_;
    std::shared_ptr<SignalIN> robot_state_input_signal_;
    callback_t callback_function_;
    std::thread thread_;
    real_time_tools::Spinner spinner_;
    bool stop_publish_;
    std::mutex mutex_;
    dynamicgraph::Vector robot_state_;

    ~RosRobotStatePublisherMtInternal()
    {
        stop_publish_ = true;
        thread_.join();
    }
};

class RosRobotStatePublisherMt : public dynamicgraph::Entity
{
public:
    RosRobotStatePublisherMt(const std::string& name);

    ~RosRobotStatePublisherMt(void)
    {
    }

    void add(const std::string& base_link_name,
             const std::string& joint_names,
             const std::string& tf_prefix,
             const std::string& signal_name,
             const std::string& joint_state_topic_name);

    int& trigger(int&, int);

    void copy_data_internally(
        std::shared_ptr<RosRobotStatePublisherMtInternal> publisher,
        int dg_time);

    void send_data(RosRobotStatePublisherMtInternal& publisher);

private:
    static const std::string CLASS_NAME;

    virtual const std::string& getClassName(void) const
    {
        return CLASS_NAME;
    }

    void set_tf_msg_to_identity(geometry_msgs::TransformStamped& msg);

    void normalize_tf_msg_quaternion(geometry_msgs::TransformStamped& msg);

    SignalOUT trigger_signal_;

    ros::NodeHandle& ros_node_handle_;

    std::map<std::string, std::shared_ptr<RosRobotStatePublisherMtInternal>>
        publishers_;

    ros::Duration rate_;

    ros::Time last_time_of_publication_;
};
}  // namespace dynamic_graph

namespace dynamicgraph
{
namespace command
{
namespace ros_state_publish
{
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;

// Define here a macro that automatically generates a command
#define ROS_PUBLISH_MAKE_COMMAND(CMD)                        \
    class CMD : public Command                               \
    {                                                        \
    public:                                                  \
        CMD(dynamic_graph::RosRobotStatePublisherMt& entity, \
            const std::string& docstring);                   \
        virtual Value doExecute();                           \
    }

// Generate a couple of command classes
ROS_PUBLISH_MAKE_COMMAND(Add);
// Later in the code no one must have access to this macro.
#undef ROS_PUBLISH_MAKE_COMMAND
}  // end of namespace ros_state_publish
}  // end of namespace command
}  // end of namespace dynamicgraph

#endif  // ROS_ROBOT_STATE_PUBLISHER_HPP