diff --git a/sdformat_test_files/CMakeLists.txt b/sdformat_test_files/CMakeLists.txt
index 66e87ffe..510bde71 100644
--- a/sdformat_test_files/CMakeLists.txt
+++ b/sdformat_test_files/CMakeLists.txt
@@ -34,6 +34,7 @@ set(model_names
"joint_fixed"
"joint_gearbox"
"joint_prismatic"
+ "joint_prismatic_no_axis"
"joint_revolute"
"joint_revolute2"
"joint_revolute_axis"
diff --git a/sdformat_test_files/models/joint_prismatic_no_axis/joint_prismatic_no_axis.sdf b/sdformat_test_files/models/joint_prismatic_no_axis/joint_prismatic_no_axis.sdf
new file mode 100644
index 00000000..39c17575
--- /dev/null
+++ b/sdformat_test_files/models/joint_prismatic_no_axis/joint_prismatic_no_axis.sdf
@@ -0,0 +1,65 @@
+
+
+
+
+
+
+
+ 0.1 0.2 0.4
+
+
+
+
+
+
+ 0.1 0.2 0.4
+
+
+
+
+ 12.3
+
+ 0.205
+ 0
+ 0
+ 0.17425
+ 0
+ 0.05125
+
+
+
+
+ 0.1 0 0.1 0 0 0
+
+
+
+ 0.1 0.2 0.3
+
+
+
+
+
+
+ 0.1 0.2 0.3
+
+
+
+
+ 1.23
+
+ 0.013325
+ 0
+ 0
+ 0.01025
+ 0
+ 0.005125
+
+
+
+
+
+ link_1
+ link_2
+
+
+
diff --git a/sdformat_test_files/models/joint_prismatic_no_axis/model.config b/sdformat_test_files/models/joint_prismatic_no_axis/model.config
new file mode 100644
index 00000000..2ceb5977
--- /dev/null
+++ b/sdformat_test_files/models/joint_prismatic_no_axis/model.config
@@ -0,0 +1,19 @@
+
+
+ joint_prismatic_no_axis
+ 1.0
+ joint_prismatic_no_axis.sdf
+
+
+ Shane Loretz
+ sloretz@openrobotics.org
+
+
+ Dharini Dutia
+ dharini@openrobotics.org
+
+
+
+ A model with two links connected by a prismatic joint having no axis described.
+
+
diff --git a/sdformat_urdf/CMakeLists.txt b/sdformat_urdf/CMakeLists.txt
index c5ec1faa..d2e87211 100644
--- a/sdformat_urdf/CMakeLists.txt
+++ b/sdformat_urdf/CMakeLists.txt
@@ -101,6 +101,7 @@ if(BUILD_TESTING)
sdformat_test_files_get_model_sdf("path_to_sdf_joint_fixed" "joint_fixed")
sdformat_test_files_get_model_sdf("path_to_sdf_joint_gearbox" "joint_gearbox")
sdformat_test_files_get_model_sdf("path_to_sdf_joint_prismatic" "joint_prismatic")
+ sdformat_test_files_get_model_sdf("path_to_sdf_joint_prismatic_no_axis" "joint_prismatic_no_axis")
sdformat_test_files_get_model_sdf("path_to_sdf_joint_revolute" "joint_revolute")
sdformat_test_files_get_model_sdf("path_to_sdf_joint_revolute2" "joint_revolute2")
sdformat_test_files_get_model_sdf("path_to_sdf_joint_revolute_axis" "joint_revolute_axis")
diff --git a/sdformat_urdf/src/sdformat_urdf.cpp b/sdformat_urdf/src/sdformat_urdf.cpp
index 9abed32a..0bca5841 100644
--- a/sdformat_urdf/src/sdformat_urdf.cpp
+++ b/sdformat_urdf/src/sdformat_urdf.cpp
@@ -494,12 +494,14 @@ sdformat_urdf::convert_joint(const sdf::Joint & sdf_joint, sdf::Errors & errors)
case sdf::JointType::PRISMATIC:
urdf_joint->type = urdf::Joint::PRISMATIC;
break;
- case sdf::JointType::INVALID: // Unsupported: fall through to default
- case sdf::JointType::BALL: // |
+ case sdf::JointType::UNIVERSAL: // Unsupported: fall through to floating
+ case sdf::JointType::BALL: // Will require custom TF publisher
case sdf::JointType::GEARBOX: // |
case sdf::JointType::REVOLUTE2: // |
- case sdf::JointType::SCREW: // |
- case sdf::JointType::UNIVERSAL: // V
+ case sdf::JointType::SCREW: // V
+ urdf_joint->type = urdf::Joint::FLOATING;
+ break;
+ case sdf::JointType::INVALID:
default:
errors.emplace_back(
sdf::ErrorCode::STRING_READ,
@@ -507,9 +509,15 @@ sdformat_urdf::convert_joint(const sdf::Joint & sdf_joint, sdf::Errors & errors)
return nullptr;
}
- if (urdf::Joint::FIXED != urdf_joint->type) {
- // Add axis info for non-fixed joints
+ if ((urdf::Joint::FIXED != urdf_joint->type) && (urdf::Joint::FLOATING != urdf_joint->type)) {
+ // Add axis info for non-fixed and non-floating joints
const sdf::JointAxis * sdf_axis = sdf_joint.Axis(0);
+ if (nullptr == sdf_axis) {
+ errors.emplace_back(
+ sdf::ErrorCode::STRING_READ,
+ "Axes missing for joint [" + sdf_joint.Name() + "]");
+ return nullptr;
+ }
// URDF expects axis to be expressed in the joint frame
ignition::math::Vector3d axis_xyz;
diff --git a/sdformat_urdf/test/joint_tests.cpp b/sdformat_urdf/test/joint_tests.cpp
index 91e917f4..afc36b09 100644
--- a/sdformat_urdf/test/joint_tests.cpp
+++ b/sdformat_urdf/test/joint_tests.cpp
@@ -12,7 +12,6 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-
#include
#include
#include
@@ -28,9 +27,21 @@ TEST(Joint, joint_ball)
sdf::Errors errors;
urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse(
get_file(PATH_TO_SDF_JOINT_BALL), errors);
- EXPECT_FALSE(errors.empty());
+ EXPECT_TRUE(errors.empty()) << errors;
EXPECT_NO_ALGORITHM_ERRORS(errors);
- ASSERT_FALSE(model);
+ ASSERT_TRUE(model);
+ ASSERT_EQ("joint_ball", model->getName());
+
+ urdf::JointConstSharedPtr joint = model->getJoint("joint_ball");
+ ASSERT_NE(nullptr, joint);
+
+ EXPECT_EQ("joint_ball", joint->name);
+ EXPECT_EQ(urdf::Joint::FLOATING, joint->type);
+ ASSERT_EQ(nullptr, joint->dynamics);
+ ASSERT_EQ(nullptr, joint->limits);
+ ASSERT_EQ(nullptr, joint->safety);
+ ASSERT_EQ(nullptr, joint->calibration);
+ ASSERT_EQ(nullptr, joint->mimic);
}
TEST(Joint, joint_continuous)
@@ -120,6 +131,15 @@ TEST(Joint, joint_prismatic)
ASSERT_EQ(nullptr, joint->mimic);
}
+TEST(Joint, joint_prismatic_no_axis)
+{
+ sdf::Errors errors;
+ urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse(
+ get_file(PATH_TO_SDF_JOINT_PRISMATIC_NO_AXIS), errors);
+ EXPECT_FALSE(errors.empty());
+ ASSERT_FALSE(model);
+}
+
TEST(Joint, joint_revolute)
{
sdf::Errors errors;
@@ -152,9 +172,21 @@ TEST(Joint, joint_revolute2)
sdf::Errors errors;
urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse(
get_file(PATH_TO_SDF_JOINT_REVOLUTE2), errors);
- EXPECT_FALSE(errors.empty());
+ EXPECT_TRUE(errors.empty()) << errors;
EXPECT_NO_ALGORITHM_ERRORS(errors);
- ASSERT_FALSE(model);
+ ASSERT_TRUE(model);
+ ASSERT_EQ("joint_revolute2", model->getName());
+
+ urdf::JointConstSharedPtr joint = model->getJoint("joint_revolute2");
+ ASSERT_NE(nullptr, joint);
+
+ EXPECT_EQ("joint_revolute2", joint->name);
+ EXPECT_EQ(urdf::Joint::FLOATING, joint->type);
+ ASSERT_EQ(nullptr, joint->dynamics);
+ ASSERT_EQ(nullptr, joint->limits);
+ ASSERT_EQ(nullptr, joint->safety);
+ ASSERT_EQ(nullptr, joint->calibration);
+ ASSERT_EQ(nullptr, joint->mimic);
}
TEST(Joint, joint_revolute_axis)
@@ -244,9 +276,21 @@ TEST(Joint, joint_screw)
sdf::Errors errors;
urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse(
get_file(PATH_TO_SDF_JOINT_SCREW), errors);
- EXPECT_FALSE(errors.empty());
+ EXPECT_TRUE(errors.empty()) << errors;
EXPECT_NO_ALGORITHM_ERRORS(errors);
- ASSERT_FALSE(model);
+ ASSERT_TRUE(model);
+ ASSERT_EQ("joint_screw", model->getName());
+
+ urdf::JointConstSharedPtr joint = model->getJoint("joint_screw");
+ ASSERT_NE(nullptr, joint);
+
+ EXPECT_EQ("joint_screw", joint->name);
+ EXPECT_EQ(urdf::Joint::FLOATING, joint->type);
+ ASSERT_EQ(nullptr, joint->dynamics);
+ ASSERT_EQ(nullptr, joint->limits);
+ ASSERT_EQ(nullptr, joint->safety);
+ ASSERT_EQ(nullptr, joint->calibration);
+ ASSERT_EQ(nullptr, joint->mimic);
}
TEST(Joint, joint_universal)
@@ -254,7 +298,19 @@ TEST(Joint, joint_universal)
sdf::Errors errors;
urdf::ModelInterfaceSharedPtr model = sdformat_urdf::parse(
get_file(PATH_TO_SDF_JOINT_UNIVERSAL), errors);
- EXPECT_FALSE(errors.empty());
+ EXPECT_TRUE(errors.empty()) << errors;
EXPECT_NO_ALGORITHM_ERRORS(errors);
- ASSERT_FALSE(model);
+ ASSERT_TRUE(model);
+ ASSERT_EQ("joint_universal", model->getName());
+
+ urdf::JointConstSharedPtr joint = model->getJoint("joint_universal");
+ ASSERT_NE(nullptr, joint);
+
+ EXPECT_EQ("joint_universal", joint->name);
+ EXPECT_EQ(urdf::Joint::FLOATING, joint->type);
+ ASSERT_EQ(nullptr, joint->dynamics);
+ ASSERT_EQ(nullptr, joint->limits);
+ ASSERT_EQ(nullptr, joint->safety);
+ ASSERT_EQ(nullptr, joint->calibration);
+ ASSERT_EQ(nullptr, joint->mimic);
}
diff --git a/sdformat_urdf/test/sdf_paths.hpp.in b/sdformat_urdf/test/sdf_paths.hpp.in
index 75f62bfd..8cae9fc0 100644
--- a/sdformat_urdf/test/sdf_paths.hpp.in
+++ b/sdformat_urdf/test/sdf_paths.hpp.in
@@ -35,6 +35,7 @@
#define PATH_TO_SDF_JOINT_FIXED "@path_to_sdf_joint_fixed@"
#define PATH_TO_SDF_JOINT_GEARBOX "@path_to_sdf_joint_gearbox@"
#define PATH_TO_SDF_JOINT_PRISMATIC "@path_to_sdf_joint_prismatic@"
+#define PATH_TO_SDF_JOINT_PRISMATIC_NO_AXIS "@path_to_sdf_joint_prismatic_no_axis@"
#define PATH_TO_SDF_JOINT_REVOLUTE "@path_to_sdf_joint_revolute@"
#define PATH_TO_SDF_JOINT_REVOLUTE2 "@path_to_sdf_joint_revolute2@"
#define PATH_TO_SDF_JOINT_REVOLUTE_AXIS "@path_to_sdf_joint_revolute_axis@"