diff --git a/robot_state_publisher/CMakeLists.txt b/robot_state_publisher/CMakeLists.txt index b3220fe..0dc2111 100644 --- a/robot_state_publisher/CMakeLists.txt +++ b/robot_state_publisher/CMakeLists.txt @@ -31,8 +31,19 @@ rosbuild_add_executable(test_publisher test/test_publisher.cpp ) target_link_libraries(test_publisher ${PROJECT_NAME}) rosbuild_add_gtest_build_flags(test_publisher) rosbuild_declare_test(test_publisher) - rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_publisher.launch) +rosbuild_add_executable(test_two_links_fixed_joint test/test_two_links_fixed_joint.cpp ) +target_link_libraries(test_two_links_fixed_joint ${PROJECT_NAME}) +rosbuild_add_gtest_build_flags(test_two_links_fixed_joint) +rosbuild_declare_test(test_two_links_fixed_joint) +rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_fixed_joint.launch) + +rosbuild_add_executable(test_two_links_moving_joint test/test_two_links_moving_joint.cpp ) +target_link_libraries(test_two_links_moving_joint ${PROJECT_NAME}) +rosbuild_add_gtest_build_flags(test_two_links_moving_joint) +rosbuild_declare_test(test_two_links_moving_joint) +rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_moving_joint.launch) + # Download needed data file rosbuild_download_test_data(http://pr.willowgarage.com/data/robot_state_publisher/joint_states_indexed.bag test/joint_states_indexed.bag 793e0b566ebe4698265a936b92fa2bba) diff --git a/robot_state_publisher/src/joint_state_listener.cpp b/robot_state_publisher/src/joint_state_listener.cpp index 268117b..69df07a 100644 --- a/robot_state_publisher/src/joint_state_listener.cpp +++ b/robot_state_publisher/src/joint_state_listener.cpp @@ -64,7 +64,7 @@ JointStateListener::~JointStateListener() void JointStateListener::callbackJointState(const JointStateConstPtr& state) { - if (state->get_name_size() != state->get_position_size()){ + if (state->name.size() != state->position.size()){ ROS_ERROR("Robot state publisher received an invalid joint state vector"); return; } diff --git a/robot_state_publisher/src/robot_state_publisher.cpp b/robot_state_publisher/src/robot_state_publisher.cpp index 221fe2e..ffb0405 100644 --- a/robot_state_publisher/src/robot_state_publisher.cpp +++ b/robot_state_publisher/src/robot_state_publisher.cpp @@ -68,7 +68,7 @@ bool RobotStatePublisher::publishTransforms(const map& joint_pos // calculate transforms form root to every segment in tree map link_poses; solver_->JntToCart(joint_positions, link_poses); - if (link_poses.size() < 2){ + if (link_poses.empty()){ ROS_ERROR("Could not compute link poses. The tree or the state is invalid."); return false; } diff --git a/robot_state_publisher/src/treefksolverposfull_recursive.cpp b/robot_state_publisher/src/treefksolverposfull_recursive.cpp index 7fdf560..fd6fcd3 100644 --- a/robot_state_publisher/src/treefksolverposfull_recursive.cpp +++ b/robot_state_publisher/src/treefksolverposfull_recursive.cpp @@ -54,7 +54,7 @@ void TreeFkSolverPosFull_recursive::addFrameToMap(const map& q_i const Frame& previous_frame, const SegmentMap::const_iterator this_segment) { // get pose of this segment - Frame this_frame = previous_frame; + Frame this_frame; double jnt_p = 0; if (this_segment->second.segment.getJoint().getType() != Joint::None){ map::const_iterator jnt_pos = q_in.find(this_segment->second.segment.getJoint().getName()); @@ -64,7 +64,9 @@ void TreeFkSolverPosFull_recursive::addFrameToMap(const map& q_i } jnt_p = jnt_pos->second; } - this_frame = this_frame * this_segment->second.segment.pose(jnt_p); + this_frame = previous_frame * this_segment->second.segment.pose(jnt_p); + double r, p, y; + this_frame.M.GetRPY(r, p, y); if (this_segment->first != tree.getRootSegment()->first) p_out.insert(make_pair(this_segment->first, this_frame)); diff --git a/robot_state_publisher/test/one_link.urdf b/robot_state_publisher/test/one_link.urdf new file mode 100644 index 0000000..be21ee1 --- /dev/null +++ b/robot_state_publisher/test/one_link.urdf @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/robot_state_publisher/test/pr2.urdf b/robot_state_publisher/test/pr2.urdf index 81f8d79..07308a0 100644 --- a/robot_state_publisher/test/pr2.urdf +++ b/robot_state_publisher/test/pr2.urdf @@ -1,1032 +1,988 @@ - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + - - + 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 - - + + power_state + 10.0 + 87.78 + -474 + 525 + 15.52 + 16.41 + - + - + + - + + - + + - + + - + + - - - - - + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -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 - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 + 640 + 1 + 0.0 0.0 0.0 + false + -129.998394137 + 129.998394137 + 0.05 + 10.0 + 0.01 + 20 + + 0.005 + true + 20 + base_scan + base_laser_link + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + true + + base_link_geom + 100.0 + 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_link - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + base_bumper + + + + + + + + true + 100.0 + base_link + base_pose_ground_truth + 0.01 + map + 25.7 25.7 0 + + 0 0 0 + + + base_footprint + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - torso_lift_link_geom - 100.0 - - true - 100.0 - torso_lift_bumper - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + torso_lift_link_geom + 100.0 + true 100.0 - torso_lift_link - imu_data - 0.01 - map - 0 0 0 - 0 0 0 - - - - - - -52143.33 - - + torso_lift_bumper + + + + + + + + + -52143.33 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 100.0 + imu_link + torso_lift_imu/data + 2.89e-08 + 0 0 0 + 0 0 0 + + + + numerical values that were specified in common.xacro which was included above --> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 6.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 6.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 6.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 6.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + R8G8B8 2448 2050 @@ -1034,2344 +990,2387 @@ 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 + /prosilica/image_raw + /prosilica/camera_info + /prosilica/request_image + high_def_frame 1224.5 1224.5 1025.5 - 2955 - 0 - 0 - 0 - 0 - 0 + 2955 + + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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 - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + BAYER_BGGR8 + 90 + 0.1 + 100 + 25.0 + true - 20.0 - wide_stereo_l_sensor - wide_stereo_r_sensor - wide_stereo/raw_stereo + 25.0 + wide_stereo/left/image_raw + wide_stereo/left/camera_info 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 - - - - - - + 0 + 320.5 + 320.5 + 240.5 + + + 320 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + BAYER_BGGR8 + 90 + 0.1 + 100 + 25.0 + true - 20.0 - narrow_stereo_l_sensor - narrow_stereo_r_sensor - narrow_stereo/raw_stereo + 25.0 + wide_stereo/right/image_raw + wide_stereo/right/camera_info + wide_stereo_optical_frame + 0.09 + 320.5 + 320.5 + 240.5 + + + 320 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 45 + 0.1 + 100 + 25.0 + + true + 25.0 + narrow_stereo/left/image_raw + narrow_stereo/left/camera_info narrow_stereo_optical_frame - 320 - 320 - 240 - 772.55 - 0 - 0 - 0 - 0 - 0 - -0.09 - - - - - - + 0 + 320.5 + 320.5 + 240.5 + + + 772.55 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 45 + 0.1 + 100 + 25.0 + + true + 25.0 + narrow_stereo/right/image_raw + narrow_stereo/right/camera_info + narrow_stereo_optical_frame + 0.09 + 320.5 + 320.5 + 240.5 + + + 772.55 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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 - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 + 640 + 1 + 0.0 0.0 0.0 + false + -79.9999999086 + 79.9999999086 + 0.05 + 10.0 + 0.01 + 40 + + 0.005 + true + 40 + tilt_scan + laser_tilt_link + + + + + + + + + + + -6.05 + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - r_shoulder_pan_link_geom + + + + + + + + true + + + + + + + + + + 32.6525111499 + + + + r_shoulder_pan_link_geom + 100.0 + + true 100.0 - - true - 100.0 - r_shoulder_pan_bumper - - - - - true - - - - - - r_shoulder_lift_link_geom + r_shoulder_pan_bumper + + + + + true + + + + + + + + r_shoulder_lift_link_geom + 100.0 + + true 100.0 - - true - 100.0 - r_r_shoulder_lift_bumper - - - - true - - - - - - true - - - - - - - r_upper_arm_link_geom + r_r_shoulder_lift_bumper + + + + true + + + + + + + + + 63.1552452977 + + + + + 61.8948225713 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + r_upper_arm_link_geom + + 100.0 + + true 100.0 - - true - 100.0 - r_upper_arm_bumper - - - - true - - - - r_elbow_flex_link_geom + r_upper_arm_bumper + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + -90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -1.0 + + + + r_elbow_flex_link_geom + 100.0 + + true 100.0 - - true - 100.0 - r_elbow_flex_bumper - - - - - true - - - - - - true - - - - true - - r_forearm_link_geom + r_elbow_flex_bumper + + + + + true + + + + + + + + + -36.167452007 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + r_forearm_link_geom + 100.0 + + true 100.0 - - true - 100.0 - r_forearm_bumper - - - - - - true - - r_wrist_flex_link_geom + r_forearm_bumper + + + + + + true + + r_wrist_flex_link_geom + 100.0 + + true 100.0 - - true - 100.0 - r_wrist_flex_bumper - - - - - - - - - true - - r_wrist_roll_link_geom + r_wrist_flex_bumper + + + + + + + + + + + true + + r_wrist_roll_link_geom + 100.0 + + true 100.0 - - true - 100.0 - r_wrist_roll_bumper - - - - - - - - - - 63.16 - - - - 61.89 - - - - 32.65 - - - - -36.17 - - - - 90.5142857143 - - - - - - + r_wrist_roll_bumper + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - +--> + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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 + + r_gripper_l_finger_link_geom + 100.0 + true 100.0 - r_gripper_l_finger_link - r_gripper_l_finger_pose_ground_truth - 0.0 - map - - - + r_gripper_l_finger_bumper + + + + + + + + + + + + + + + + + + true + + r_gripper_r_finger_link_geom + 100.0 + 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 - - - - + r_gripper_r_finger_bumper + + + + + + + + + + + + + + + + + true + false + + r_gripper_l_finger_tip_link_geom + 100.0 + true 100.0 - r_gripper_palm_link - r_gripper_palm_pose_ground_truth - 0 0 0 - 0 0 0 - 0.0 - map - - - + r_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + + + true + false + + r_gripper_r_finger_tip_link_geom + 100.0 + true 100.0 - r_gripper_tool_frame - r_gripper_tool_frame_pose_ground_truth - 0 0 0 - 0 0 0 - 0.0 - /map - - - - true - - - - - - - - - - + r_gripper_r_finger_tip_bumper + + + + + + + + + + + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_pose_ground_truth + 0.0 + base_link + + + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_force_ground_truth + r_gripper_l_finger_link + + + + + + + + + + + + r_gripper_r_parallel_link + r_gripper_r_finger_tip_link + r_gripper_r_finger_tip_link + 0 0 1 + -0.018 -0.021 0 + + + r_gripper_l_parallel_link + r_gripper_l_finger_tip_link + r_gripper_l_finger_tip_link + 0 0 1 + -0.018 0.021 0 + + + r_gripper_l_finger_tip_link + r_gripper_r_finger_tip_link + r_gripper_r_finger_tip_link + 0 1 0 + + + + true + + + + true + + + + + + + + + + true + + r_gripper_palm_link_geom + 100.0 + + true + 100.0 + r_gripper_palm_bumper + + + + + + + + true + 100.0 + r_gripper_palm_link + r_gripper_palm_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + map + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - l_shoulder_pan_link_geom + + + + + + + + true + + + + + + + + + + 32.6525111499 + + + + l_shoulder_pan_link_geom + 100.0 + + true 100.0 - - true - 100.0 - l_shoulder_pan_bumper - - - - - true - - - - - - l_shoulder_lift_link_geom + l_shoulder_pan_bumper + + + + + true + + + + + + + + l_shoulder_lift_link_geom + 100.0 + + true 100.0 - - true - 100.0 - l_r_shoulder_lift_bumper - - - - true - - - - - - true - - - - - - - l_upper_arm_link_geom + l_r_shoulder_lift_bumper + + + + true + + + + + + + + + 63.1552452977 + + + + + 61.8948225713 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + l_upper_arm_link_geom + + 100.0 + + true 100.0 - - true - 100.0 - l_upper_arm_bumper - - - - true - - - - l_elbow_flex_link_geom + l_upper_arm_bumper + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + -90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -1.0 + + + + l_elbow_flex_link_geom + 100.0 + + true 100.0 - - true - 100.0 - l_elbow_flex_bumper - - - - - true - - - - - - true - - - - true - - l_forearm_link_geom + l_elbow_flex_bumper + + + + + true + + + + + + + + + -36.167452007 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + l_forearm_link_geom + 100.0 + + true 100.0 - - true - 100.0 - l_forearm_bumper - - - - - - true - - l_wrist_flex_link_geom + l_forearm_bumper + + + + + + true + + l_wrist_flex_link_geom + 100.0 + + true 100.0 - - true - 100.0 - l_wrist_flex_bumper - - - - - - - - - true - - l_wrist_roll_link_geom + l_wrist_flex_bumper + + + + + + + + + + + true + + l_wrist_roll_link_geom + 100.0 + + true 100.0 - - true - 100.0 - l_wrist_roll_bumper - - - - - - - - - - 63.16 - - - - 61.89 - - - - 32.65 - - - - -36.17 - - - - 90.5142857143 - - - - - - + l_wrist_roll_bumper + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - +--> + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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 + + l_gripper_l_finger_link_geom + 100.0 + true 100.0 - l_gripper_l_finger_link - l_gripper_l_finger_pose_ground_truth - 0.0 - map - - - + l_gripper_l_finger_bumper + + + + + + + + + + + + + + + + + + true + + l_gripper_r_finger_link_geom + 100.0 + 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 - - - - + l_gripper_r_finger_bumper + + + + + + + + + + + + + + + + + true + false + + l_gripper_l_finger_tip_link_geom + 100.0 + true 100.0 - l_gripper_palm_link - l_gripper_palm_pose_ground_truth - 0 0 0 - 0 0 0 - 0.0 - map - - - + l_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + + + true + false + + l_gripper_r_finger_tip_link_geom + 100.0 + true 100.0 - l_gripper_tool_frame - l_gripper_tool_frame_pose_ground_truth - 0 0 0 - 0 0 0 - 0.0 - /map - - - - true - - - - - - - - - - - + l_gripper_r_finger_tip_bumper + + + + + + + + + + + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_pose_ground_truth + 0.0 + base_link + + + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_force_ground_truth + l_gripper_l_finger_link + + + + + + + + + + + + l_gripper_r_parallel_link + l_gripper_r_finger_tip_link + l_gripper_r_finger_tip_link + 0 0 1 + -0.018 -0.021 0 + + + l_gripper_l_parallel_link + l_gripper_l_finger_tip_link + l_gripper_l_finger_tip_link + 0 0 1 + -0.018 0.021 0 + + + l_gripper_l_finger_tip_link + l_gripper_r_finger_tip_link + l_gripper_r_finger_tip_link + 0 1 0 + + + + true + + + + true + + + + + + + + + + true + + l_gripper_palm_link_geom + 100.0 + + true + 100.0 + l_gripper_palm_bumper + + + + + + + + true + 100.0 + l_gripper_palm_link + l_gripper_palm_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + map + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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 + 25.0 + + true + 25.0 + l_forearm_cam/image_raw + l_forearm_cam/camera_info + l_forearm_cam_optical_frame + 0 + 320.5 + 320.5 + 240.5 + + + 320 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 640 480 - L8 - 90 - 0.1 - 100 - 20.0 - - true - 20.0 - r_forearm_cam/image - r_forearm_cam_frame - - - - true - PR2/Blue - - true - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 90 + 0.1 + 100 + 25.0 + + true + 25.0 + r_forearm_cam/image_raw + r_forearm_cam/camera_info + r_forearm_cam_optical_frame + 0 + 320.5 + 320.5 + 240.5 + + + 320 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + diff --git a/robot_state_publisher/test/test_publisher.cpp b/robot_state_publisher/test/test_publisher.cpp index 41c96a7..530170e 100644 --- a/robot_state_publisher/test/test_publisher.cpp +++ b/robot_state_publisher/test/test_publisher.cpp @@ -62,31 +62,11 @@ public: protected: /// constructor TestPublisher() - { - /* - // constructs a robot model from the xml file - urdf::Model robot_model; - if (g_argc == 2){ - if (!robot_model.initFile(g_argv[1])) - ROS_ERROR("Failed to construct robot model from xml string"); - } - else - ROS_ERROR("No robot model as argument given"); - - // constructs a kdl tree from the robot model - Tree tree; - if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) - ROS_ERROR("Failed to extract kdl tree from robot model"); - - publisher = new JointStateListener(tree); - */ - } + {} /// Destructor ~TestPublisher() - { - // delete publisher; - } + {} }; diff --git a/robot_state_publisher/test/test_two_links_fixed_joint.cpp b/robot_state_publisher/test/test_two_links_fixed_joint.cpp new file mode 100644 index 0000000..f0355c0 --- /dev/null +++ b/robot_state_publisher/test/test_two_links_fixed_joint.cpp @@ -0,0 +1,120 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#include +#include +#include +#include +#include +#include +#include +#include "robot_state_publisher/joint_state_listener.h" + + +using namespace ros; +using namespace tf; +using namespace robot_state_publisher; + + +int g_argc; +char** g_argv; + +#define EPS 0.01 + +class TestPublisher : public testing::Test +{ +public: + JointStateListener* publisher; + +protected: + /// constructor + TestPublisher() + {} + + /// Destructor + ~TestPublisher() + {} +}; + + + + + +TEST_F(TestPublisher, test) +{ + ROS_INFO("Creating tf listener"); + TransformListener tf; + + ROS_INFO("Publishing joint state to robot state publisher"); + ros::NodeHandle n; + ros::Publisher js_pub = n.advertise("joint_states", 100); + sensor_msgs::JointState js_msg; + for (unsigned int i=0; i<100; i++){ + js_msg.header.stamp = ros::Time::now(); + js_pub.publish(js_msg); + ros::Duration(0.1).sleep(); + } + + ASSERT_TRUE(tf.canTransform("link1", "link2", Time())); + ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time())); + + tf::StampedTransform t; + tf.lookupTransform("link1", "link2",Time(), t ); + EXPECT_NEAR(t.getOrigin().x(), 5.0, EPS); + EXPECT_NEAR(t.getOrigin().y(), 0.0, EPS); + EXPECT_NEAR(t.getOrigin().z(), 0.0, EPS); + + SUCCEED(); +} + + + + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_robot_state_publisher"); + ros::NodeHandle node; + boost::thread ros_thread(boost::bind(&ros::spin)); + + g_argc = argc; + g_argv = argv; + int res = RUN_ALL_TESTS(); + ros_thread.interrupt(); + ros_thread.join(); + + return res; +} diff --git a/robot_state_publisher/test/test_two_links_fixed_joint.launch b/robot_state_publisher/test/test_two_links_fixed_joint.launch new file mode 100644 index 0000000..9ad67b0 --- /dev/null +++ b/robot_state_publisher/test/test_two_links_fixed_joint.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/robot_state_publisher/test/test_two_links_moving_joint.cpp b/robot_state_publisher/test/test_two_links_moving_joint.cpp new file mode 100644 index 0000000..2f05f79 --- /dev/null +++ b/robot_state_publisher/test/test_two_links_moving_joint.cpp @@ -0,0 +1,122 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Wim Meeussen */ + +#include +#include +#include +#include +#include +#include +#include +#include "robot_state_publisher/joint_state_listener.h" + + +using namespace ros; +using namespace tf; +using namespace robot_state_publisher; + + +int g_argc; +char** g_argv; + +#define EPS 0.01 + +class TestPublisher : public testing::Test +{ +public: + JointStateListener* publisher; + +protected: + /// constructor + TestPublisher() + {} + + /// Destructor + ~TestPublisher() + {} +}; + + + + + +TEST_F(TestPublisher, test) +{ + ROS_INFO("Creating tf listener"); + TransformListener tf; + + ROS_INFO("Publishing joint state to robot state publisher"); + ros::NodeHandle n; + ros::Publisher js_pub = n.advertise("joint_states", 100); + sensor_msgs::JointState js_msg; + js_msg.name.push_back("joint1"); + js_msg.position.push_back(M_PI); + for (unsigned int i=0; i<100; i++){ + js_msg.header.stamp = ros::Time::now(); + js_pub.publish(js_msg); + ros::Duration(0.1).sleep(); + } + + ASSERT_TRUE(tf.canTransform("link1", "link2", Time())); + ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time())); + + tf::StampedTransform t; + tf.lookupTransform("link1", "link2",Time(), t ); + EXPECT_NEAR(t.getOrigin().x(), 5.0, EPS); + EXPECT_NEAR(t.getOrigin().y(), 0.0, EPS); + EXPECT_NEAR(t.getOrigin().z(), 0.0, EPS); + + SUCCEED(); +} + + + + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_robot_state_publisher"); + ros::NodeHandle node; + boost::thread ros_thread(boost::bind(&ros::spin)); + + g_argc = argc; + g_argv = argv; + int res = RUN_ALL_TESTS(); + ros_thread.interrupt(); + ros_thread.join(); + + return res; +} diff --git a/robot_state_publisher/test/test_two_links_moving_joint.launch b/robot_state_publisher/test/test_two_links_moving_joint.launch new file mode 100644 index 0000000..51a4b55 --- /dev/null +++ b/robot_state_publisher/test/test_two_links_moving_joint.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/robot_state_publisher/test/two_links_fixed_joint.urdf b/robot_state_publisher/test/two_links_fixed_joint.urdf new file mode 100644 index 0000000..8bd84ef --- /dev/null +++ b/robot_state_publisher/test/two_links_fixed_joint.urdf @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/robot_state_publisher/test/two_links_moving_joint.urdf b/robot_state_publisher/test/two_links_moving_joint.urdf new file mode 100644 index 0000000..eb50dd6 --- /dev/null +++ b/robot_state_publisher/test/two_links_moving_joint.urdf @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file