maxstaehr

attach model to link in gazebo

Feb 27th, 2013
215
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <boost/bind.hpp>
  2. #include <gazebo.hh>
  3. #include <physics/physics.hh>
  4. #include <common/common.hh>
  5. #include <stdio.h>
  6. #include <map>
  7.  
  8. #include "ros/ros.h"
  9. #include "std_msgs/Float64.h"
  10. #include "sensor_msgs/JointState.h"
  11. #include "st2_ca/PltfState.h"
  12. #include "MMKR16/SetArmPose.h"
  13. #include "MMKR16/SetCameraLinkPose.h"
  14. #include "MMKR16/SetPltfPose.h"
  15. #include "MMKR16/SetCameraLink.h"
  16.  
  17.  
  18. namespace gazebo {
  19. class ROSModelPlugin: public ModelPlugin {
  20.  
  21. public:
  22.     ROSModelPlugin() {
  23.         // Start up ROS
  24.         std::string name = "ros_model_plugin_node";
  25.         int argc = 0;
  26.         ros::init(argc, NULL, name);
  27.         printf("\n\n\n\nSensorState test\n\n\n\n");
  28.         is_model_attached = false;
  29.         pltfPoseHasChanged = false;
  30.  
  31.     }
  32. public:
  33.     ~ROSModelPlugin() {
  34.         delete this->node;
  35.     }
  36.  
  37. public:
  38.     bool setArmPose(MMKR16::SetArmPose::Request  &req,
  39.             MMKR16::SetArmPose::Response &res)
  40.     {
  41.         armPoseLock.lock();
  42.         av[0] = req.a1;
  43.         av[1] = req.a2;
  44.         av[2] = req.a3;
  45.         av[3] = req.a4;
  46.         av[4] = req.a5;
  47.         av[5] = req.a6;
  48.         armPoseLock.unlock();
  49.         std::cout << "set Arm Pose call" << std::endl;
  50.         res.succeeded = true;
  51.         return true;
  52.     }
  53.  
  54.     bool setCameraLink(MMKR16::SetCameraLink::Request  &req,
  55.             MMKR16::SetCameraLink::Response &res)
  56.     {
  57.         cameraLinkLock.lock();
  58.         cameraLinkHasChanged = true;
  59.         linkname = req.link;
  60.         cameraLinkLock.unlock();
  61.         std::cout << "set Camera Link call" << std::endl;
  62.         res.succeeded = true;
  63.         return true;
  64.     }
  65.  
  66.     bool setCameraLinkPose(MMKR16::SetCameraLinkPose::Request  &req,
  67.             MMKR16::SetCameraLinkPose::Response &res)
  68.     {
  69.         cameraLinkPoseLock.lock();
  70.         math::Vector3 vec(req.x, req.y, req.z);
  71.         math::Quaternion quad(req.w,req.wx, req.wy,req.wz);
  72.         cameraPose.Set(vec, quad);
  73.         cameraLinkPoseLock.unlock();
  74.         std::cout << "set Camera Link Pose call" << std::endl;
  75.         res.succeeded = true;
  76.         return true;
  77.     }
  78.  
  79.     bool setPltfPose(MMKR16::SetPltfPose::Request  &req,
  80.             MMKR16::SetPltfPose::Response &res)
  81.     {
  82.         pltfPoseLock.lock();
  83.         pltfPoseHasChanged = true;
  84.         worldPose.Set(req.x, req.y,0,0,0,req.theta);
  85.         pltfPoseLock.unlock();
  86.         std::cout << "set Pltf Pose call" << std::endl;
  87.         res.succeeded = true;
  88.         return true;
  89.     }
  90.  
  91. public:
  92.     void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) {
  93.         // Store the pointer to the model
  94.         this->model = _parent;
  95.  
  96.         //Initialize Joints
  97.         this->axis1 = this->model->GetJoint("mmkr16::axis1");
  98.         this->axis2 = this->model->GetJoint("mmkr16::axis2");
  99.         this->axis3 = this->model->GetJoint("mmkr16::axis3");
  100.         this->axis4 = this->model->GetJoint("mmkr16::axis4");
  101.         this->axis5 = this->model->GetJoint("mmkr16::axis5");
  102.         this->axis6 = this->model->GetJoint("mmkr16::axis6");
  103.  
  104.         for(int i=0;i<6;i++)
  105.             av[i] = 0;
  106.  
  107.         //Initialize Link
  108.         this->baseLink = this->model->GetLink("base_link");
  109.         this->link1 = this->model->GetLink("link1");
  110.         this->link2 = this->model->GetLink("link2");
  111.         this->link3 = this->model->GetLink("link3");
  112.         this->link4 = this->model->GetLink("link4");
  113.         this->link5 = this->model->GetLink("link5");
  114.         this->link6 = this->model->GetLink("link6");
  115.  
  116.         attachedLink = this->model->GetLink("link3");
  117.         cameraModel = physics::get_world()->GetModel("camera_model");
  118.         cameraPose.Set(1,1,1,0,0,0);
  119.  
  120.         // ROS Nodehandle
  121.         this->node = new ros::NodeHandle("~");
  122.  
  123.  
  124.         SetArmPoseService = node->advertiseService("SetArmPose", &ROSModelPlugin::setArmPose, this);
  125.         SetPltfPoseService = node->advertiseService("SetPltfPose", &ROSModelPlugin::setPltfPose, this);
  126.         SetCameraLinkPoseService = node->advertiseService("SetCameraLinkPose", &ROSModelPlugin::setCameraLinkPose, this);
  127.         SetCameraLinkService = node->advertiseService("SetCameraLink", &ROSModelPlugin::setCameraLink, this);
  128.  
  129.         // Listen to the update event. This event is broadcast every
  130.         // simulation iteration.
  131.         this->updateConnection = event::Events::ConnectWorldUpdateStart(
  132.                 boost::bind(&ROSModelPlugin::OnUpdate, this));
  133.  
  134.     }
  135.  
  136.     // Called by the world update start event
  137. public:
  138.     void OnUpdate() {
  139.  
  140.  
  141.         // let ros run
  142.         ros::spinOnce();
  143.  
  144.         armPoseLock.lock();
  145.         this->axis1->SetAngle(0, av[0]);
  146.         this->axis2->SetAngle(0, av[1]);
  147.         this->axis3->SetAngle(0, av[2]);
  148.         this->axis4->SetAngle(0, av[3]);
  149.         this->axis5->SetAngle(0, av[4]);
  150.         this->axis6->SetAngle(0, av[5]);
  151.         armPoseLock.unlock();
  152.  
  153.  
  154.  
  155.         pltfPoseLock.lock();
  156.         if(pltfPoseHasChanged)
  157.         {
  158.             bool is_paused = this->model->GetWorld()->IsPaused();
  159.             this->model->GetWorld()->SetPaused(true);
  160.             this->model->SetWorldPose(worldPose);
  161.             this->model->GetWorld()->SetPaused(is_paused);
  162.             pltfPoseHasChanged = false;
  163.             std::cout << "Pltf Pose set" << std::endl;
  164.         }
  165.         pltfPoseLock.unlock();
  166.  
  167.  
  168.         cameraLinkLock.lock();
  169.         if(cameraLinkHasChanged)
  170.         {
  171.             bool is_paused = this->model->GetWorld()->IsPaused();
  172.             this->model->GetWorld()->SetPaused(true);
  173.  
  174.             std::cout << "attach link to: "<< linkname << std::endl;
  175.             if(linkname.compare("base_link")==0)
  176.             {
  177.                 attachedLink = baseLink;
  178.                 baseLink->AttachStaticModel(cameraModel,cameraPose);
  179.             }else if(linkname.compare("link1")==0)
  180.             {
  181.                 attachedLink = link1;
  182.                 link1->AttachStaticModel(cameraModel,cameraPose);
  183.             }else if(linkname.compare("link2")==0)
  184.             {
  185.                 attachedLink = link2;
  186.                 link2->AttachStaticModel(cameraModel,cameraPose);
  187.             }else if(linkname.compare("link3")==0)
  188.             {
  189.                 attachedLink = link3;
  190.                 link3->AttachStaticModel(cameraModel,cameraPose);
  191.             }else if(linkname.compare("link4")==0)
  192.             {
  193.                 attachedLink = link4;
  194.                 link4->AttachStaticModel(cameraModel,cameraPose);
  195.             }else if(linkname.compare("link5")==0)
  196.             {
  197.                 attachedLink = link5;
  198.                 link5->AttachStaticModel(cameraModel,cameraPose);
  199.             }else if(linkname.compare("link6")==0)
  200.             {
  201.                 attachedLink = link6;
  202.                 link6->AttachStaticModel(cameraModel,cameraPose);
  203.             }else
  204.             {
  205.                 std::cout << "no link found to attach" << std::endl;
  206.             }
  207.             cameraLinkHasChanged = false;
  208.  
  209.             this->model->GetWorld()->SetPaused(is_paused);
  210.         }
  211.         cameraLinkLock.unlock();
  212.  
  213.         //cameraLinkPoseLock.lock();
  214.         //math::Pose pose = cameraPose + attachedLink->GetWorldPose();
  215.         //cameraModel->SetWorldPose(pose);
  216.         //cameraLinkPoseLock.unlock();
  217.  
  218.  
  219.  
  220.  
  221.     }
  222.  
  223.  
  224.  
  225.     // Pointer to the model
  226. private:
  227.     physics::ModelPtr model;
  228.  
  229.     // Pointer to the update event connection
  230. private:
  231.     event::ConnectionPtr updateConnection;
  232.  
  233.     // ROS Nodehandle
  234. private:
  235.     ros::NodeHandle* node;
  236.  
  237.     // Axis
  238. private:
  239.     physics::JointPtr axis1;
  240. private:
  241.     physics::JointPtr axis2;
  242. private:
  243.     physics::JointPtr axis3;
  244. private:
  245.     physics::JointPtr axis4;
  246. private:
  247.     physics::JointPtr axis5;
  248. private:
  249.     physics::JointPtr axis6;
  250.  
  251.     physics::LinkPtr baseLink, link1, link2, link3, link4,link5,link6;
  252.  
  253. private:
  254.     physics::LinkPtr attachedLink;
  255. private:
  256.     physics::ModelPtr cameraModel;
  257.  
  258.     //model attached
  259. private:
  260.     bool is_model_attached;
  261.  
  262.     //World Pose
  263. private:
  264.     math::Pose worldPose;
  265.     //Camera Pose
  266. private:
  267.  
  268. private:
  269.     math::Pose pose;
  270.  
  271. private:
  272.  
  273.     boost::mutex pltfPoseLock;
  274.     bool pltfPoseHasChanged;
  275.     boost::mutex armPoseLock;
  276.     double av[6];
  277.     boost::mutex cameraLinkLock;
  278.     bool cameraLinkHasChanged;
  279.     std::string linkname;
  280.     boost::mutex cameraLinkPoseLock;
  281.     math::Pose cameraPose;
  282.  
  283.     //string joint map
  284. private:
  285.  
  286.     ros::ServiceServer SetArmPoseService;
  287.     ros::ServiceServer SetCameraLinkPoseService;
  288.     ros::ServiceServer SetPltfPoseService;
  289.     ros::ServiceServer SetCameraLinkService;
  290.  
  291. };
  292.  
  293. // Register this plugin with the simulator
  294. GZ_REGISTER_MODEL_PLUGIN(ROSModelPlugin)
  295. }
RAW Paste Data