diff --git a/urdf/src/link.cpp b/urdf/src/link.cpp index 11aa9ea..f0adc1d 100644 --- a/urdf/src/link.cpp +++ b/urdf/src/link.cpp @@ -67,8 +67,11 @@ boost::shared_ptr parseGeometry(TiXmlElement *g) return geom; } - if (!geom->initXml(shape)) - return geom; + // clear geom object when fails to initialize + if (!geom->initXml(shape)){ + ROS_ERROR("Geometry failed to parse"); + geom.reset(); + } return geom; } @@ -365,7 +368,7 @@ bool Link::initXml(TiXmlElement* config) } name = std::string(name_char); - // Inertial + // Inertial (optional) TiXmlElement *i = config->FirstChildElement("inertial"); if (i) { @@ -373,11 +376,11 @@ bool Link::initXml(TiXmlElement* config) if (!inertial->initXml(i)) { ROS_ERROR("Could not parse inertial element for Link '%s'", this->name.c_str()); - inertial.reset(); + return false; } } - // Visual + // Visual (optional) TiXmlElement *v = config->FirstChildElement("visual"); if (v) { @@ -385,11 +388,11 @@ bool Link::initXml(TiXmlElement* config) if (!visual->initXml(v)) { ROS_ERROR("Could not parse visual element for Link '%s'", this->name.c_str()); - visual.reset(); + return false; } } - // Collision + // Collision (optional) TiXmlElement *col = config->FirstChildElement("collision"); if (col) { @@ -397,7 +400,7 @@ bool Link::initXml(TiXmlElement* config) if (!collision->initXml(col)) { ROS_ERROR("Could not parse collision element for Link '%s'", this->name.c_str()); - collision.reset(); + return false; } } diff --git a/urdf/test/pr2_desc_no_filename_in_mesh.xml b/urdf/test/pr2_desc_no_filename_in_mesh.xml new file mode 100644 index 0000000..21c98b8 --- /dev/null +++ b/urdf/test/pr2_desc_no_filename_in_mesh.xml @@ -0,0 +1,3377 @@ + + + + + + + + + + + + + + + + + + + + true + 1000.0 + + + + + + true + 1000.0 + + + + + + true + 1.0 + 5 + -10.0 + 1.0 + 10.0 + 1200000.0 + diagnostic + battery_state + self_test + + + + + true + 1000.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + fl_caster_l_wheel_link_geom + 100.0 + + true + 100.0 + fl_caster_l_wheel_bumper + + + + + + + + + + + + 75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + fl_caster_r_wheel_link_geom + 100.0 + + true + 100.0 + fl_caster_r_wheel_bumper + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + fr_caster_l_wheel_link_geom + 100.0 + + true + 100.0 + fr_caster_l_wheel_bumper + + + + + + + + + + + + 75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + fr_caster_r_wheel_link_geom + 100.0 + + true + 100.0 + fr_caster_r_wheel_bumper + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + bl_caster_l_wheel_link_geom + 100.0 + + true + 100.0 + bl_caster_l_wheel_bumper + + + + + + + + + + + + 75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + bl_caster_r_wheel_link_geom + 100.0 + + true + 100.0 + bl_caster_r_wheel_bumper + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + br_caster_l_wheel_link_geom + 100.0 + + true + 100.0 + br_caster_l_wheel_bumper + + + + + + + + + + + + 75.0676691729 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + br_caster_r_wheel_link_geom + 100.0 + + true + 100.0 + br_caster_r_wheel_bumper + + + + + + + + + + + + -75.0676691729 + + + + base_link_geom + 100.0 + + true + 100.0 + base_bumper + + + + + + + + + + base_link + + true + 100.0 + base_link + base_pose_ground_truth + 0.01 + map + 25.7 25.7 0 + 0 0 0 + + + + true + 100.0 + plug_holder + plug_holder_pose_ground_truth + 0.01 + map + 0 0 0 + 0 0 0 + + + + + 640 + 640 + 1 + 0.0 0.0 0.0 + false + + -135 + 135 + + 0.05 + 10.0 + 0.01 + 20.0 + + 0.005 + true + 20.0 + base_scan + base_laser + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + torso_lift_link_geom + 100.0 + + true + 100.0 + torso_lift_bumper + + + + + + + true + 100.0 + torso_lift_link + imu_data + 0.01 + map + 0 0 0 + 0 0 0 + + + + + + -52143.33 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 6.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 6.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + R8G8B8 + 2448 2050 + 45 + 0.1 + 100 + 20.0 + + true + 20.0 + prosilica/cam_info + prosilica/image + prosilica/image_rect + prosilica/cam_info_service + prosilica/poll + hight_def_optical_frame + 1224.5 + 1224.5 + 1025.5 + 2955 + 0 + 0 + 0 + 0 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 90 + 0.1 + 100 + 20.0 + + true + 20.0 + wide_stereo/left_image + wide_stereo_l_stereo_camera_frame + + + + + + + 640 480 + L8 + 90 + 0.1 + 100 + 20.0 + + true + 20.0 + wide_stereo/right_image + wide_stereo_r_stereo_camera_frame + + + + + + + true + 20.0 + wide_stereo_l_sensor + wide_stereo_r_sensor + wide_stereo/raw_stereo + wide_stereo_optical_frame + 320 + 320 + 240 + 320 + 0 + 0 + 0 + 0 + 0 + -0.09 + + + + + + + + + 640 480 + L8 + 45 + 0.1 + 100 + 20.0 + + true + 20.0 + narrow_stereo/left_image + narrow_stereo_l_stereo_camera_frame + + + + + + + 640 480 + L8 + 45 + 0.1 + 100 + 20.0 + + true + 20.0 + narrow_stereo/right_image + narrow_stereo_r_stereo_camera_frame + + + + + + + true + 20.0 + narrow_stereo_l_sensor + narrow_stereo_r_sensor + narrow_stereo/raw_stereo + narrow_stereo_optical_frame + 320 + 320 + 240 + 772.55 + 0 + 0 + 0 + 0 + 0 + -0.09 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 + 640 + 1 + + 0.0 0.0 0.0 + false + + -80 + 80 + + 0.05 + 10.0 + 0.01 + 40.0 + + 0.005 + true + 40.0 + tilt_scan + laser_tilt_link + + + + + + + 6.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + r_shoulder_pan_link_geom + 100.0 + + true + 100.0 + r_shoulder_pan_bumper + + + + + true + + + + + + r_shoulder_lift_link_geom + 100.0 + + true + 100.0 + r_r_shoulder_lift_bumper + + + + true + + + + + + true + + + + + + + r_upper_arm_link_geom + 100.0 + + true + 100.0 + r_upper_arm_bumper + + + + true + + + + r_elbow_flex_link_geom + 100.0 + + true + 100.0 + r_elbow_flex_bumper + + + + + true + + + + + + true + + + + true + + r_forearm_link_geom + 100.0 + + true + 100.0 + r_forearm_bumper + + + + + + true + + r_wrist_flex_link_geom + 100.0 + + true + 100.0 + r_wrist_flex_bumper + + + + + + + + + true + + r_wrist_roll_link_geom + 100.0 + + true + 100.0 + r_wrist_roll_bumper + + + + + + + + + + 63.16 + + + + 61.89 + + + + 32.65 + + + + -36.17 + + + + 90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + r_gripper_l_finger_link_geom + 100.0 + + true + 100.0 + r_gripper_l_finger_bumper + + + + + + + + + + + + + + + + true + + r_gripper_r_finger_link_geom + 100.0 + + true + 100.0 + r_gripper_r_finger_bumper + + + + + + + + + + + + + + + true + + + r_gripper_l_finger_tip_link_geom + 100.0 + + true + 100.0 + r_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + true + + + r_gripper_r_finger_tip_link_geom + 100.0 + + true + 100.0 + r_gripper_r_finger_tip_bumper + + + + + + + + + + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_pose_ground_truth + 0.0 + map + + + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_force_ground_truth + r_gripper_l_finger_link + + + + + + + + + + + + + true + + r_gripper_palm_link_geom + 100.0 + + true + 100.0 + r_gripper_palm_bumper + + + + + + true + + + + r_gripper_l_finger_tip_link + r_gripper_float_link + r_gripper_l_finger_tip_link + 0 1 0 + 0 0 0 + + + r_gripper_r_finger_tip_link + r_gripper_float_link + r_gripper_r_finger_tip_link + 0 1 0 + 0 0 0 + + + + + true + 100.0 + r_gripper_palm_link + r_gripper_palm_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + map + + + + true + 100.0 + r_gripper_tool_frame + r_gripper_tool_frame_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + /map + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + l_shoulder_pan_link_geom + 100.0 + + true + 100.0 + l_shoulder_pan_bumper + + + + + true + + + + + + l_shoulder_lift_link_geom + 100.0 + + true + 100.0 + l_r_shoulder_lift_bumper + + + + true + + + + + + true + + + + + + + l_upper_arm_link_geom + 100.0 + + true + 100.0 + l_upper_arm_bumper + + + + true + + + + l_elbow_flex_link_geom + 100.0 + + true + 100.0 + l_elbow_flex_bumper + + + + + true + + + + + + true + + + + true + + l_forearm_link_geom + 100.0 + + true + 100.0 + l_forearm_bumper + + + + + + true + + l_wrist_flex_link_geom + 100.0 + + true + 100.0 + l_wrist_flex_bumper + + + + + + + + + true + + l_wrist_roll_link_geom + 100.0 + + true + 100.0 + l_wrist_roll_bumper + + + + + + + + + + 63.16 + + + + 61.89 + + + + 32.65 + + + + -36.17 + + + + 90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + l_gripper_l_finger_link_geom + 100.0 + + true + 100.0 + l_gripper_l_finger_bumper + + + + + + + + + + + + + + + + true + + l_gripper_r_finger_link_geom + 100.0 + + true + 100.0 + l_gripper_r_finger_bumper + + + + + + + + + + + + + + + true + + + l_gripper_l_finger_tip_link_geom + 100.0 + + true + 100.0 + l_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + true + + + l_gripper_r_finger_tip_link_geom + 100.0 + + true + 100.0 + l_gripper_r_finger_tip_bumper + + + + + + + + + + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_pose_ground_truth + 0.0 + map + + + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_force_ground_truth + l_gripper_l_finger_link + + + + + + + + + + + + + true + + l_gripper_palm_link_geom + 100.0 + + true + 100.0 + l_gripper_palm_bumper + + + + + + true + + + + l_gripper_l_finger_tip_link + l_gripper_float_link + l_gripper_l_finger_tip_link + 0 1 0 + 0 0 0 + + + l_gripper_r_finger_tip_link + l_gripper_float_link + l_gripper_r_finger_tip_link + 0 1 0 + 0 0 0 + + + + + true + 100.0 + l_gripper_palm_link + l_gripper_palm_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + map + + + + true + 100.0 + l_gripper_tool_frame + l_gripper_tool_frame_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + /map + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 90 + 0.1 + 100 + 20.0 + + true + 20.0 + l_forearm_cam/image + l_forearm_cam_frame + + + + true + PR2/Blue + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 90 + 0.1 + 100 + 20.0 + + true + 20.0 + r_forearm_cam/image + r_forearm_cam_frame + + + + true + PR2/Blue + + true + + + + + + + diff --git a/urdf/test/test_robot_model_parser.launch b/urdf/test/test_robot_model_parser.launch index fc0662e..66d8df1 100644 --- a/urdf/test/test_robot_model_parser.launch +++ b/urdf/test/test_robot_model_parser.launch @@ -1,3 +1,3 @@ - +