iri_ik_fake_recorder.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include <iri_wam_common_msgs/wamInverseKinematics.h>
#include "iri_fake_recorder.h"
#include <geometry_msgs/PoseStamped.h>
#include <string>

class IKFakeRecorder : public IRIFakeRecorder<geometry_msgs::PoseStamped>
{
    private:
        ros::NodeHandle    nh_;
        ros::ServiceServer ik_cmd_server_;
        std::string        server_uri_;

        bool ik_cmdCallback(iri_wam_common_msgs::wamInverseKinematics::Request &req,
                            iri_wam_common_msgs::wamInverseKinematics::Response &res);

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

IKFakeRecorder::IKFakeRecorder(const std::string server_uri) :
    server_uri_(server_uri)
{
    ik_cmd_server_ = nh_.advertiseService(server_uri_, & IKFakeRecorder::ik_cmdCallback, this);
}

bool
IKFakeRecorder::ik_cmdCallback(iri_wam_common_msgs::wamInverseKinematics::Request & req,
                               iri_wam_common_msgs::wamInverseKinematics::Response &res)
{
    msg_recieved_history_.push_back(req.pose);

    // Simulate the seven DOF from wam
    for (int i = 0; i < 7; i++)
        res.joints.position.push_back(1.0);

    return true;
}