Do a few cleanup tasks in collada_urdf (#183)

* Style cleanup within collada_urdf.

No functional change, just style.

Signed-off-by: Chris Lalancette <clalancette@osrfoundation.org>

* Make sure to quit out of urdf_to_collada when invalid file is found.

Otherwise, we'll just end up segfaulting later on.

Signed-off-by: Chris Lalancette <clalancette@osrfoundation.org>

* Re-enable one of the collada-urdf tests.

In point of fact, we delete the rest of the tests because
they don't make much sense anymore.  Just enable this one
for now; we'll enable further ones in the future.

Signed-off-by: Chris Lalancette <clalancette@osrfoundation.org>

* Add in another test for collada_urdf.

Signed-off-by: Chris Lalancette <clalancette@osrfoundation.org>

collada_urdf/CMakeLists.txt

@@ -75,8 +75,8 @@ set_target_properties(collada_to_urdf PROPERTIES COMPILER_FLAGS "${ASSIMP_CXX_FL
 set_target_properties(collada_to_urdf PROPERTIES LINK_FLAGS "${ASSIMP_LINK_FLAGS}")
 
 if(CATKIN_ENABLE_TESTING)
-  catkin_add_gtest(test_collada_writer test/test_collada_urdf.cpp)
+  catkin_add_gtest(test_collada_urdf test/test_collada_urdf.cpp WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
-  target_link_libraries(test_collada_writer ${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES}  
+  target_link_libraries(test_collada_urdf ${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES}
     ${Boost_LIBRARIES})
 endif()
 

collada_urdf/src/collada_urdf.cpp

@@ -949,17 +949,17 @@ protected:
 
         //  Declare all the joints
         _mapjointindices.clear();
-        int index=0;
+        int index = 0;
         FOREACHC(itj, _robot.joints_) {
             _mapjointindices[itj->second] = index++;
         }
         _maplinkindices.clear();
-        index=0;
+        index = 0;
         FOREACHC(itj, _robot.links_) {
             _maplinkindices[itj->second] = index++;
         }
         _mapmaterialindices.clear();
-        index=0;
+        index = 0;
         FOREACHC(itj, _robot.materials_) {
             _mapmaterialindices[itj->second] = index++;
         }
@@ -980,8 +980,8 @@ protected:
             pdomjoint->setName(pjoint->name.c_str());
             domAxis_constraintRef axis;
             if( !!pjoint->limits ) {
-                lmin=pjoint->limits->lower;
+                lmin = pjoint->limits->lower;
-                lmax=pjoint->limits->upper;
+                lmax = pjoint->limits->upper;
             }
             else {
                 lmin = lmax = 0;
@@ -993,8 +993,8 @@ protected:
             case urdf::Joint::CONTINUOUS:
                 axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE));
                 fmult = 180.0f/M_PI;
-                lmin*=fmult;
+                lmin *= fmult;
-                lmax*=fmult;
+                lmax *= fmult;
                 break;
             case urdf::Joint::PRISMATIC:
                 axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_PRISMATIC));
@@ -1176,7 +1176,8 @@ protected:
                         if ( it != plink->visual_array.begin() ) {
                             urdf::Pose org_trans =  _poseMult(geometry_origin_inv, (*it)->origin);
                             pdomgeom = _WriteGeometry((*it)->geometry, geomid, &org_trans);
-		  } else {
+                        }
+                        else {
                             pdomgeom = _WriteGeometry((*it)->geometry, geomid);
                         }
                         domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
@@ -1576,7 +1577,8 @@ protected:
                         parray->getValue().append(nv.x);
                         parray->getValue().append(nv.y);
                         parray->getValue().append(nv.z);
-                    } else {
+                    }
+                    else {
                         parray->getValue().append(p.x*scale.x);
                         parray->getValue().append(p.y*scale.y);
                         parray->getValue().append(p.z*scale.z);
@@ -1935,7 +1937,7 @@ ColladaUrdfException::ColladaUrdfException(std::string const& what)
 }
 
 bool WriteUrdfModelToColladaFile(urdf::Model const& robot_model, string const& file) {
-    ColladaWriter writer(robot_model,0);
+    ColladaWriter writer(robot_model, 0);
     if ( ! writer.convert() ) {
         std::cerr << std::endl << "Error converting document" << std::endl;
         return -1;

collada_urdf/src/urdf_to_collada.cpp

@@ -57,7 +57,8 @@ int main(int argc, char** argv)
 
     urdf::Model robot_model;
     if( !robot_model.initFile(input_filename) ) {
-        ROS_ERROR("failed to open urdf file %s",input_filename.c_str());
+        ROS_ERROR("failed to open urdf file %s", input_filename.c_str());
+        return -2;
     }
 
     collada_urdf::WriteUrdfModelToColladaFile(robot_model, output_filename);

collada_urdf/test/test_collada_urdf.cpp

@@ -34,52 +34,23 @@
 #include <fstream>
 #include <sstream>
 #include <string>
-/*
-std::string readTestUrdfString() {
-    std::ifstream file("test/pr2.urdf");
-    std::stringstream ss;
-    ss << file.rdbuf();
-    return ss.str();
-}
 
 TEST(collada_urdf, collada_from_urdf_file_works)
 {
-    boost::shared_ptr<DAE> dom;
+    urdf::Model robot_model;
-    ASSERT_TRUE(collada_urdf::colladaFromUrdfFile("test/pr2.urdf", dom));
+
-    ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae"));
+    ASSERT_TRUE(robot_model.initFile("pr2.urdf"));
+    ASSERT_TRUE(collada_urdf::WriteUrdfModelToColladaFile(robot_model, "pr2.dae"));
 }
 
-TEST(collada_urdf, collada_from_urdf_string_works)
+TEST(collada_urdf, collada_output_dir_does_not_exist)
-{
-    std::string urdf_str = readTestUrdfString();
-
-    boost::shared_ptr<DAE> dom;
-    ASSERT_TRUE(collada_urdf::colladaFromUrdfString(urdf_str, dom));
-    ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae"));
-}
-
-TEST(collada_urdf, collada_from_urdf_xml_works)
-{
-    TiXmlDocument urdf_xml;
-    ASSERT_TRUE(urdf_xml.Parse(readTestUrdfString().c_str()) > 0);
-
-    boost::shared_ptr<DAE> dom;
-    ASSERT_TRUE(collada_urdf::colladaFromUrdfXml(&urdf_xml, dom));
-    ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae"));
-}
-
-TEST(collada_urdf, collada_from_urdf_model_works)
 {
     urdf::Model robot_model;
-    TiXmlDocument urdf_xml;
-    ASSERT_TRUE(urdf_xml.Parse(readTestUrdfString().c_str()) > 0);
-    ASSERT_TRUE(robot_model.initXml(&urdf_xml));
 
-    boost::shared_ptr<DAE> dom;
+    ASSERT_TRUE(robot_model.initFile("pr2.urdf"));
-    ASSERT_TRUE(collada_urdf::colladaFromUrdfModel(robot_model, dom));
+    ASSERT_FALSE(collada_urdf::WriteUrdfModelToColladaFile(robot_model, "a/very/long/directory/path/that/should/not/exist/pr2.dae"));
-    ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae"));
 }
-*/
+
 int main(int argc, char **argv) {
     testing::InitGoogleTest(&argc, argv);
     return RUN_ALL_TESTS();