iri_wam_joint_move_fake_recorder.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "iri_fake_recorder.h"
#include <iri_wam_common_msgs/joints_move.h>
#include <string>
#include <cstring>

template<size_t DOF>
class WamJointMoveFakeRecorder : public IRIFakeRecorder<std::vector<double> >
{
    private:
        ros::NodeHandle    nh_;
        ros::ServiceServer joints_move_server_;
        std::string        server_uri_;
        size_t             dof_;


        bool joints_moveCallback(iri_wam_common_msgs::joints_move::Request &req,
                                 iri_wam_common_msgs::joints_move::Response &res);

    public:
        WamJointMoveFakeRecorder(const std::string server_uri);
};

template<size_t DOF>
WamJointMoveFakeRecorder<DOF>::WamJointMoveFakeRecorder(const std::string server_uri) :
    server_uri_(server_uri),
    dof_(DOF)
{
    joints_move_server_ = nh_.advertiseService(server_uri_, &WamJointMoveFakeRecorder::joints_moveCallback, 
                                               this);
}

template<size_t DOF>
bool
WamJointMoveFakeRecorder<DOF>::joints_moveCallback(iri_wam_common_msgs::joints_move::Request &req,
                                                   iri_wam_common_msgs::joints_move::Response &res)
{
    if (req.joints.size() != dof_)
    {
        std::cerr << "Invalid number of joints for moving: " << req.joints.size()
                  << " Expected: " << dof_;
        return false;
    }

    msg_recieved_history_.push_back(req.joints);
    return true;
}