attach model to link in gazebo

#include <boost/bind.hpp>
#include <gazebo.hh>
#include <physics/physics.hh>
#include <common/common.hh>
#include <stdio.h>
#include <map>

#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include "sensor_msgs/JointState.h"
#include "st2_ca/PltfState.h"
#include "MMKR16/SetArmPose.h"
#include "MMKR16/SetCameraLinkPose.h"
#include "MMKR16/SetPltfPose.h"
#include "MMKR16/SetCameraLink.h"


namespace gazebo {
class ROSModelPlugin: public ModelPlugin {

public:
    ROSModelPlugin() {
        // Start up ROS
        std::string name = "ros_model_plugin_node";
        int argc = 0;
        ros::init(argc, NULL, name);
        printf("\n\n\n\nSensorState test\n\n\n\n");
        is_model_attached = false;
        pltfPoseHasChanged = false;

    }
public:
    ~ROSModelPlugin() {
        delete this->node;
    }

public:
    bool setArmPose(MMKR16::SetArmPose::Request  &req,
            MMKR16::SetArmPose::Response &res)
    {
        armPoseLock.lock();
        av[0] = req.a1;
        av[1] = req.a2;
        av[2] = req.a3;
        av[3] = req.a4;
        av[4] = req.a5;
        av[5] = req.a6;
        armPoseLock.unlock();
        std::cout << "set Arm Pose call" << std::endl;
        res.succeeded = true;
        return true;
    }

    bool setCameraLink(MMKR16::SetCameraLink::Request  &req,
            MMKR16::SetCameraLink::Response &res)
    {
        cameraLinkLock.lock();
        cameraLinkHasChanged = true;
        linkname = req.link;
        cameraLinkLock.unlock();
        std::cout << "set Camera Link call" << std::endl;
        res.succeeded = true;
        return true;
    }

    bool setCameraLinkPose(MMKR16::SetCameraLinkPose::Request  &req,
            MMKR16::SetCameraLinkPose::Response &res)
    {
        cameraLinkPoseLock.lock();
        math::Vector3 vec(req.x, req.y, req.z);
        math::Quaternion quad(req.w,req.wx, req.wy,req.wz);
        cameraPose.Set(vec, quad);
        cameraLinkPoseLock.unlock();
        std::cout << "set Camera Link Pose call" << std::endl;
        res.succeeded = true;
        return true;
    }

    bool setPltfPose(MMKR16::SetPltfPose::Request  &req,
            MMKR16::SetPltfPose::Response &res)
    {
        pltfPoseLock.lock();
        pltfPoseHasChanged = true;
        worldPose.Set(req.x, req.y,0,0,0,req.theta);
        pltfPoseLock.unlock();
        std::cout << "set Pltf Pose call" << std::endl;
        res.succeeded = true;
        return true;
    }

public:
    void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) {
        // Store the pointer to the model
        this->model = _parent;

        //Initialize Joints
        this->axis1 = this->model->GetJoint("mmkr16::axis1");
        this->axis2 = this->model->GetJoint("mmkr16::axis2");
        this->axis3 = this->model->GetJoint("mmkr16::axis3");
        this->axis4 = this->model->GetJoint("mmkr16::axis4");
        this->axis5 = this->model->GetJoint("mmkr16::axis5");
        this->axis6 = this->model->GetJoint("mmkr16::axis6");

        for(int i=0;i<6;i++)
            av[i] = 0;

        //Initialize Link
        this->baseLink = this->model->GetLink("base_link");
        this->link1 = this->model->GetLink("link1");
        this->link2 = this->model->GetLink("link2");
        this->link3 = this->model->GetLink("link3");
        this->link4 = this->model->GetLink("link4");
        this->link5 = this->model->GetLink("link5");
        this->link6 = this->model->GetLink("link6");

        attachedLink = this->model->GetLink("link3");
        cameraModel = physics::get_world()->GetModel("camera_model");
        cameraPose.Set(1,1,1,0,0,0);

        // ROS Nodehandle
        this->node = new ros::NodeHandle("~");


        SetArmPoseService = node->advertiseService("SetArmPose", &ROSModelPlugin::setArmPose, this);
        SetPltfPoseService = node->advertiseService("SetPltfPose", &ROSModelPlugin::setPltfPose, this);
        SetCameraLinkPoseService = node->advertiseService("SetCameraLinkPose", &ROSModelPlugin::setCameraLinkPose, this);
        SetCameraLinkService = node->advertiseService("SetCameraLink", &ROSModelPlugin::setCameraLink, this);

        // Listen to the update event. This event is broadcast every
        // simulation iteration.
        this->updateConnection = event::Events::ConnectWorldUpdateStart(
                boost::bind(&ROSModelPlugin::OnUpdate, this));

    }

    // Called by the world update start event
public:
    void OnUpdate() {


        // let ros run
        ros::spinOnce();

        armPoseLock.lock();
        this->axis1->SetAngle(0, av[0]);
        this->axis2->SetAngle(0, av[1]);
        this->axis3->SetAngle(0, av[2]);
        this->axis4->SetAngle(0, av[3]);
        this->axis5->SetAngle(0, av[4]);
        this->axis6->SetAngle(0, av[5]);
        armPoseLock.unlock();



        pltfPoseLock.lock();
        if(pltfPoseHasChanged)
        {
            bool is_paused = this->model->GetWorld()->IsPaused();
            this->model->GetWorld()->SetPaused(true);
            this->model->SetWorldPose(worldPose);
            this->model->GetWorld()->SetPaused(is_paused);
            pltfPoseHasChanged = false;
            std::cout << "Pltf Pose set" << std::endl;
        }
        pltfPoseLock.unlock();


        cameraLinkLock.lock();
        if(cameraLinkHasChanged)
        {
            bool is_paused = this->model->GetWorld()->IsPaused();
            this->model->GetWorld()->SetPaused(true);

            std::cout << "attach link to: "<< linkname << std::endl;
            if(linkname.compare("base_link")==0)
            {
                attachedLink = baseLink;
                baseLink->AttachStaticModel(cameraModel,cameraPose);
            }else if(linkname.compare("link1")==0)
            {
                attachedLink = link1;
                link1->AttachStaticModel(cameraModel,cameraPose);
            }else if(linkname.compare("link2")==0)
            {
                attachedLink = link2;
                link2->AttachStaticModel(cameraModel,cameraPose);
            }else if(linkname.compare("link3")==0)
            {
                attachedLink = link3;
                link3->AttachStaticModel(cameraModel,cameraPose);
            }else if(linkname.compare("link4")==0)
            {
                attachedLink = link4;
                link4->AttachStaticModel(cameraModel,cameraPose);
            }else if(linkname.compare("link5")==0)
            {
                attachedLink = link5;
                link5->AttachStaticModel(cameraModel,cameraPose);
            }else if(linkname.compare("link6")==0)
            {
                attachedLink = link6;
                link6->AttachStaticModel(cameraModel,cameraPose);
            }else
            {
                std::cout << "no link found to attach" << std::endl;
            }
            cameraLinkHasChanged = false;

            this->model->GetWorld()->SetPaused(is_paused);
        }
        cameraLinkLock.unlock();

        //cameraLinkPoseLock.lock();
        //math::Pose pose = cameraPose + attachedLink->GetWorldPose();
        //cameraModel->SetWorldPose(pose);
        //cameraLinkPoseLock.unlock();




    }



    // Pointer to the model
private:
    physics::ModelPtr model;

    // Pointer to the update event connection
private:
    event::ConnectionPtr updateConnection;

    // ROS Nodehandle
private:
    ros::NodeHandle* node;

    // Axis
private:
    physics::JointPtr axis1;
private:
    physics::JointPtr axis2;
private:
    physics::JointPtr axis3;
private:
    physics::JointPtr axis4;
private:
    physics::JointPtr axis5;
private:
    physics::JointPtr axis6;

    physics::LinkPtr baseLink, link1, link2, link3, link4,link5,link6;

private:
    physics::LinkPtr attachedLink;
private:
    physics::ModelPtr cameraModel;

    //model attached
private:
    bool is_model_attached;

    //World Pose
private:
    math::Pose worldPose;
    //Camera Pose
private:

private:
    math::Pose pose;

private:

    boost::mutex pltfPoseLock;
    bool pltfPoseHasChanged;
    boost::mutex armPoseLock;
    double av[6];
    boost::mutex cameraLinkLock;
    bool cameraLinkHasChanged;
    std::string linkname;
    boost::mutex cameraLinkPoseLock;
    math::Pose cameraPose;

    //string joint map
private:

    ros::ServiceServer SetArmPoseService;
    ros::ServiceServer SetCameraLinkPoseService;
    ros::ServiceServer SetPltfPoseService;
    ros::ServiceServer SetCameraLinkService;

};

// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(ROSModelPlugin)
}