code clenaup #4

ADVRHumanoids · Jan 8, 2019 · 9628089 · 9628089
1 parent 2bdcb13
commit 9628089
Showing 1 changed file with 7 additions and 36 deletions.
diff --git a/src/CartesianMarker.cpp b/src/CartesianMarker.cpp
@@ -625,8 +625,14 @@ visualization_msgs::Marker CartesianMarker::makeSTL( visualization_msgs::Interac
     }
 
     T = getPose(controlled_link->name, link->name);
+    KDL::Frame T_marker;
+    URDFPoseToKDLFrame(link->visual->origin, T_marker);
+    T = T*T_marker;
+    KDLFrameToVisualizationPose(T, _marker);
 
-
+    _marker.color.r = 0.5;
+    _marker.color.g = 0.5;
+    _marker.color.b = 0.5;
 
     if(link->visual->geometry->type == urdf::Geometry::MESH)
     {
@@ -636,18 +642,6 @@ visualization_msgs::Marker CartesianMarker::makeSTL( visualization_msgs::Interac
                 boost::static_pointer_cast<urdf::Mesh>(link->visual->geometry);
 
         _marker.mesh_resource = mesh->filename;
-
-
-        KDL::Frame T_marker;
-        URDFPoseToKDLFrame(link->visual->origin, T_marker);
-        T = T*T_marker;
-
-        KDLFrameToVisualizationPose(T, _marker);
-
-        _marker.color.r = 0.5;
-        _marker.color.g = 0.5;
-        _marker.color.b = 0.5;
-
         _marker.scale.x = mesh->scale.x;
         _marker.scale.y = mesh->scale.y;
         _marker.scale.z = mesh->scale.z;
@@ -662,14 +656,6 @@ visualization_msgs::Marker CartesianMarker::makeSTL( visualization_msgs::Interac
         KDL::Frame T_marker;
         URDFPoseToKDLFrame(link->visual->origin, T_marker);
 
-        T = T*T_marker;
-
-        KDLFrameToVisualizationPose(T, _marker);
-
-        _marker.color.r = 0.5;
-        _marker.color.g = 0.5;
-        _marker.color.b = 0.5;
-
         _marker.scale.x = mesh->dim.x;
         _marker.scale.y = mesh->dim.y;
         _marker.scale.z = mesh->dim.z;
@@ -685,13 +671,6 @@ visualization_msgs::Marker CartesianMarker::makeSTL( visualization_msgs::Interac
         KDL::Frame T_marker;
         URDFPoseToKDLFrame(link->visual->origin, T_marker);
 
-        T = T*T_marker;
-        KDLFrameToVisualizationPose(T, _marker);
-
-        _marker.color.r = 0.5;
-        _marker.color.g = 0.5;
-        _marker.color.b = 0.5;
-
         _marker.scale.x = mesh->radius;
         _marker.scale.y = mesh->radius;
         _marker.scale.z = mesh->length;
@@ -706,14 +685,6 @@ visualization_msgs::Marker CartesianMarker::makeSTL( visualization_msgs::Interac
         KDL::Frame T_marker;
         URDFPoseToKDLFrame(link->visual->origin, T_marker);
 
-        T = T*T_marker;
-
-        KDLFrameToVisualizationPose(T, _marker);
-
-        _marker.color.r = 0.5;
-        _marker.color.g = 0.5;
-        _marker.color.b = 0.5;
-
         _marker.scale.x = 2.*mesh->radius;
         _marker.scale.y = 2.*mesh->radius;
         _marker.scale.z = 2.*mesh->radius;