Check joint type

robotology · Jan 28, 2020 · 99485d1 · 99485d1
1 parent 6917e05
commit 99485d1
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 5 deletions.
diff --git a/plugins/linkattacher/include/GazeboYarpPlugins/linkattacherserverimpl.h b/plugins/linkattacher/include/GazeboYarpPlugins/linkattacherserverimpl.h
@@ -26,6 +26,11 @@
 #include <LinkAttacherServer.h>
 
 const std::string LogPrefix = "LinkAttacher:";
+// available joint types in SDF (http://sdformat.org/spec?ver=1.6&elem=joint#joint_type)
+// with the exception of gearbox joint
+const std::vector<std::string> jointTypes { "revolute", "revolute2",
+                                            "prismatic", "ball", "screw",
+                                            "universal", "fixed"};
 
 class LinkAttacherServerImpl: public GazeboYarpPlugins::LinkAttacherServer
 {
@@ -74,10 +79,7 @@ class LinkAttacherServerImpl: public GazeboYarpPlugins::LinkAttacherServer
     _model=p;
   }
 
-  void setJointType(const std::string j)
-  {
-      jointType=j;
-  }
+  bool setJointType(const std::string j);
 
 };
 

diff --git a/plugins/linkattacher/src/linkattacher.cc b/plugins/linkattacher/src/linkattacher.cc
@@ -62,7 +62,11 @@ void LinkAttacher::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
   {
       jointType="fixed";
   }
-  m_la_server.setJointType(jointType);
+
+  if(!m_la_server.setJointType(jointType))
+  {
+      return;
+  }
 
   std::string portname;
   if(m_parameters.check("name"))

diff --git a/plugins/linkattacher/src/linkattacherserverimpl.cpp b/plugins/linkattacher/src/linkattacherserverimpl.cpp
@@ -6,6 +6,9 @@
 
 #include <linkattacherserverimpl.h>
 
+#include <algorithm>
+#include <vector>
+
 using namespace gazebo;
 using namespace std;
 using namespace yarp::os;
@@ -100,6 +103,8 @@ bool LinkAttacherServerImpl::attachUnscoped(const string& parent_model_name, con
     joint = _world->GetPhysicsEngine()->CreateJoint(jointType,parent_model);
     #endif
 
+    yInfo() << "joint created!";
+
     if(!joint)
     {
         yError() << LogPrefix << "Attach: Unable to create joint";
@@ -214,3 +219,18 @@ bool LinkAttacherServerImpl::enableGravity(const string& model_name, const bool
 
   return true;
 }
+
+bool LinkAttacherServerImpl::setJointType(const std::string j)
+{
+    if(std::find(jointTypes.begin(), jointTypes.end(), j) != jointTypes.end())
+    {
+        jointType=j;
+    }
+    else
+    {
+        yError() << LogPrefix << "the choosen joint type [" << j << "] is not valid";
+        return false;
+    }
+
+    return true;
+}