VirtualServo.hpp

#ifndef VIRTUALSERVO_HPP
#define VIRTUALSERVO_HPP

#include <string>

#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include "urdf/model.h"

namespace HLR
{
namespace Simulation
{
class VirtualServo
{
public:
    VirtualServo(const urdf::JointConstSharedPtr& joint_shared_ptr,
                 const std::string& robot_description);

    void start_update_thread();

    void notify_change();

    bool is_update_received() const;

    void set_speed(double servo_speed);

    void set_target_pos(double servo_target_pos);

    void stop_movement();

    double get_current_pos() const;

    double get_max_speed() const;

    double get_max_rad() const;

    double get_min_rad() const;

    bool is_moving() const;

private:
    bool use_speed();

    double steps_to_target_time() const;

    double steps_to_target_speed() const;

    void set_servo(double pos);

    void update_servo();

    std::string servo_name;

    std::string ros_namespace;

    double current_pos;

    double target_pos;

    double speed;

    double max_speed;

    double min_rad;

    double max_rad;

    bool update_received;

    bool moving;

    ros::NodeHandle n;

    ros::Publisher joint_pub;

    int8_t not_set;

    uint8_t update_rate;

    long cmd_time;

    ros::Rate rate;
};
}
}

#endif /* VIRTUALSERVO_HPP */