ros_robot_state_publisher.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 <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>

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 RosRobotStatePublisherInternal
{
    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_;
};

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

    ~RosRobotStatePublisher(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 send_data(const RosRobotStatePublisherInternal& publisher, int time);

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, RosRobotStatePublisherInternal> 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::RosRobotStatePublisher& 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