From 81207d86d9b6968147c6630e10fbf2d63f9e1201 Mon Sep 17 00:00:00 2001 From: Dave Feil-Seifer Date: Sat, 13 Mar 2021 01:52:22 -0800 Subject: [PATCH 01/10] fixed unit tests for compile/testing in focal/noetic/opencv4 --- image_cb_detector/CMakeLists.txt | 2 +- laser_cb_detector/CMakeLists.txt | 2 +- .../laser_cb_detector/laser_cb_detector.h | 2 +- .../test/laser_cb_detector_unittest.cpp | 40 +++++++++++-------- 4 files changed, 27 insertions(+), 19 deletions(-) diff --git a/image_cb_detector/CMakeLists.txt b/image_cb_detector/CMakeLists.txt index 0c15fc8..92448f5 100644 --- a/image_cb_detector/CMakeLists.txt +++ b/image_cb_detector/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(image_cb_detector) -find_package(Boost REQUIRED thread signals) +find_package(Boost REQUIRED thread) find_package(catkin REQUIRED actionlib actionlib_msgs calibration_msgs cv_bridge genmsg geometry_msgs image_transport message_filters roscpp sensor_msgs std_msgs) find_package(OpenCV REQUIRED) diff --git a/laser_cb_detector/CMakeLists.txt b/laser_cb_detector/CMakeLists.txt index e571694..458e0f2 100644 --- a/laser_cb_detector/CMakeLists.txt +++ b/laser_cb_detector/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(laser_cb_detector) -find_package(Boost REQUIRED signals thread) +find_package(Boost REQUIRED thread) find_package(catkin REQUIRED actionlib actionlib_msgs cv_bridge image_cb_detector message_filters roscpp settlerlib std_msgs) add_action_files(DIRECTORY action FILES Config.action) diff --git a/laser_cb_detector/include/laser_cb_detector/laser_cb_detector.h b/laser_cb_detector/include/laser_cb_detector/laser_cb_detector.h index 94486d7..bd9d00c 100644 --- a/laser_cb_detector/include/laser_cb_detector/laser_cb_detector.h +++ b/laser_cb_detector/include/laser_cb_detector/laser_cb_detector.h @@ -37,7 +37,7 @@ #ifndef LASER_CB_DETECTOR_LASER_CB_DETECTOR_H_ #define LASER_CB_DETECTOR_LASER_CB_DETECTOR_H_ -#include +#include #include #include #include diff --git a/laser_cb_detector/test/laser_cb_detector_unittest.cpp b/laser_cb_detector/test/laser_cb_detector_unittest.cpp index 0dcb1f9..64f0aa5 100644 --- a/laser_cb_detector/test/laser_cb_detector_unittest.cpp +++ b/laser_cb_detector/test/laser_cb_detector_unittest.cpp @@ -37,7 +37,10 @@ #include #include -#include +#include +#include +#include + using namespace laser_cb_detector; using namespace std; @@ -55,25 +58,29 @@ static const string test_path = xstr(TEST_PATH); calibration_msgs::DenseLaserSnapshot getSnapshot(const string& filename) { - IplImage* image; - image = cvLoadImage(filename.c_str(), 0); // 0 -> Force image to grayscale - EXPECT_TRUE(image) << "could not open image file [" << filename << "]" << endl; + //IplImage* image; + cv::Mat image; + image = cv::imread(filename.c_str(), 0); // 0 -> Force image to grayscale + EXPECT_TRUE(!image.empty()) << "could not open image file [" << filename << "]" << endl; + + cv::Mat float_image; + //float_image = cv::createImage( cvSize(image->width, image->height), IPL_DEPTH_32F, 1); + //cv::convert(image, float_image); + image.convertTo(float_image, CV_32FC1, 1/255.0); - IplImage* float_image; - float_image = cvCreateImage( cvSize(image->width, image->height), IPL_DEPTH_32F, 1); - cvConvert(image, float_image); calibration_msgs::DenseLaserSnapshot snapshot; - snapshot.readings_per_scan = image->width; - snapshot.num_scans = image->height; - snapshot.intensities.resize(image->height * image->width); + snapshot.readings_per_scan = image.cols; + snapshot.num_scans = image.rows; + snapshot.intensities.resize(image.rows * image.cols); - for (int i=0; iheight; i++) + for (int i=0; iimageData + i*float_image->widthStep), - sizeof(float)*snapshot.readings_per_scan); + snapshot.intensities.assign(float_image.data, float_image.data + float_image.total()); + //memcpy(&snapshot.intensities[i*snapshot.readings_per_scan], + // (float_image.imageData + i*float_image.widthStep), + // sizeof(float)*snapshot.readings_per_scan); } if (DEBUG) @@ -91,8 +98,9 @@ calibration_msgs::DenseLaserSnapshot getSnapshot(const string& filename) } } - cvReleaseImage(&float_image); - cvReleaseImage(&image); + // since statically declared, does not need release, but this could cause a memory leak + //cv::releaseImage(&float_image); + //cv::releaseImage(&image); return snapshot; } From cf36b6c31fb719b4560a3199bee0238bbe642ba3 Mon Sep 17 00:00:00 2001 From: Dave Feil-Seifer Date: Sat, 13 Mar 2021 02:14:18 -0800 Subject: [PATCH 02/10] updated package.xml file for python3 dependencies --- calibration_estimation/package.xml | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/calibration_estimation/package.xml b/calibration_estimation/package.xml index 70d08a4..b0f9d9e 100644 --- a/calibration_estimation/package.xml +++ b/calibration_estimation/package.xml @@ -1,4 +1,4 @@ - + calibration_estimation 0.10.14 @@ -13,15 +13,17 @@ catkin - calibration_msgs - python-matplotlib - python_orocos_kdl - python-scipy - rospy - rostest - sensor_msgs - urdfdom_py - visualization_msgs + calibration_msgs + python-matplotlib + python3-matplotlib + + python_orocos_kdl + python-scipy + rospy + rostest + sensor_msgs + urdfdom_py + visualization_msgs From 089992fed91c496b9a2933caf88ba5bf39bf1e03 Mon Sep 17 00:00:00 2001 From: Dave Feil-Seifer Date: Sat, 13 Mar 2021 02:15:34 -0800 Subject: [PATCH 03/10] spell exec correctly --- calibration_estimation/package.xml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/calibration_estimation/package.xml b/calibration_estimation/package.xml index b0f9d9e..e1f9a3e 100644 --- a/calibration_estimation/package.xml +++ b/calibration_estimation/package.xml @@ -19,11 +19,11 @@ python_orocos_kdl python-scipy - rospy - rostest - sensor_msgs - urdfdom_py - visualization_msgs + rospy + rostest + sensor_msgs + urdfdom_py + visualization_msgs From 4f4edad7eb12e24513c7852a7c3c76ec565e98e0 Mon Sep 17 00:00:00 2001 From: Dave Feil-Seifer Date: Sat, 13 Mar 2021 02:18:54 -0800 Subject: [PATCH 04/10] only depend on python_orocos_kdl for python2 --- calibration_estimation/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibration_estimation/package.xml b/calibration_estimation/package.xml index e1f9a3e..8922788 100644 --- a/calibration_estimation/package.xml +++ b/calibration_estimation/package.xml @@ -16,8 +16,8 @@ calibration_msgs python-matplotlib python3-matplotlib + python_orocos_kdl - python_orocos_kdl python-scipy rospy rostest From 7013f6189ea870f243e5b9be7b1290ad8b587cda Mon Sep 17 00:00:00 2001 From: Dave Feil-Seifer Date: Sat, 13 Mar 2021 02:22:08 -0800 Subject: [PATCH 05/10] more python3 package fixes --- calibration_estimation/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/calibration_estimation/package.xml b/calibration_estimation/package.xml index 8922788..20283bf 100644 --- a/calibration_estimation/package.xml +++ b/calibration_estimation/package.xml @@ -17,8 +17,10 @@ python-matplotlib python3-matplotlib python_orocos_kdl + python3_pykdl + python-scipy + python3-scipy - python-scipy rospy rostest sensor_msgs From 7752026e089038bc28b37389e649f06b53d699ef Mon Sep 17 00:00:00 2001 From: Dave Feil-Seifer Date: Sat, 13 Mar 2021 02:23:30 -0800 Subject: [PATCH 06/10] more python3 package fixes --- calibration_estimation/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibration_estimation/package.xml b/calibration_estimation/package.xml index 20283bf..640c613 100644 --- a/calibration_estimation/package.xml +++ b/calibration_estimation/package.xml @@ -17,7 +17,7 @@ python-matplotlib python3-matplotlib python_orocos_kdl - python3_pykdl + python3-pykdl python-scipy python3-scipy From 209f44d17e62c81c2fe274eca012d293155c6f66 Mon Sep 17 00:00:00 2001 From: Dave Feil-Seifer Date: Tue, 16 Mar 2021 01:04:03 -0700 Subject: [PATCH 07/10] fixed opencv issue with laser_cb test --- laser_cb_detector/test/laser_cb_detector_unittest.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/laser_cb_detector/test/laser_cb_detector_unittest.cpp b/laser_cb_detector/test/laser_cb_detector_unittest.cpp index 64f0aa5..bd405b6 100644 --- a/laser_cb_detector/test/laser_cb_detector_unittest.cpp +++ b/laser_cb_detector/test/laser_cb_detector_unittest.cpp @@ -41,6 +41,7 @@ #include #include +#include using namespace laser_cb_detector; using namespace std; @@ -73,11 +74,14 @@ calibration_msgs::DenseLaserSnapshot getSnapshot(const string& filename) calibration_msgs::DenseLaserSnapshot snapshot; snapshot.readings_per_scan = image.cols; snapshot.num_scans = image.rows; - snapshot.intensities.resize(image.rows * image.cols); + snapshot.intensities.resize(0); + //snapshot.intensities.resize(image.rows * image.cols); + //snapshot.intensities.assign(float_image.data, float_image.data + float_image.total()); for (int i=0; i(i), float_image.ptr(i)+float_image.cols); + //snapshot.intensities.assign(float_image.data, float_image.data + float_image.total()); //memcpy(&snapshot.intensities[i*snapshot.readings_per_scan], // (float_image.imageData + i*float_image.widthStep), // sizeof(float)*snapshot.readings_per_scan); @@ -178,6 +182,8 @@ TEST(LaserCbDetector, easy_cb_3x4) snapshot = getSnapshot( test_path + "/data/cb_3x4.png"); + ROS_INFO( "test_path: [%s]/[/data/cb_3x4.png]", test_path.c_str()); + ASSERT_EQ(snapshot.readings_per_scan, (unsigned int) 303); ASSERT_EQ(snapshot.num_scans, (unsigned int) 325); From c740ce4b7e7a297acbc4e0ee4894fc8a9d4c7c99 Mon Sep 17 00:00:00 2001 From: Dave Feil-Seifer Date: Tue, 11 May 2021 10:53:27 -0700 Subject: [PATCH 08/10] fixes for compiler warnings CMP0048 and CMP0046 --- calibration_msgs/CMakeLists.txt | 4 ++-- image_cb_detector/CMakeLists.txt | 10 +++++----- interval_intersection/CMakeLists.txt | 12 ++++++------ interval_intersection/test/CMakeLists.txt | 1 + joint_states_settler/CMakeLists.txt | 6 +++--- laser_cb_detector/CMakeLists.txt | 6 +++--- laser_cb_detector/test/CMakeLists.txt | 2 +- monocam_settler/CMakeLists.txt | 2 +- settlerlib/CMakeLists.txt | 2 +- 9 files changed, 23 insertions(+), 22 deletions(-) diff --git a/calibration_msgs/CMakeLists.txt b/calibration_msgs/CMakeLists.txt index a3a8aa0..b20d425 100644 --- a/calibration_msgs/CMakeLists.txt +++ b/calibration_msgs/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(calibration_msgs) -find_package(catkin REQUIRED genmsg geometry_msgs sensor_msgs std_msgs) +find_package(catkin REQUIRED COMPONENTS message_generation geometry_msgs sensor_msgs std_msgs) add_message_files(DIRECTORY msg FILES CalibrationPattern.msg @@ -20,4 +20,4 @@ add_message_files(DIRECTORY msg generate_messages(DEPENDENCIES geometry_msgs sensor_msgs std_msgs) -catkin_package(DEPENDS geometry_msgs sensor_msgs std_msgs) +catkin_package(DEPENDS message_runtime geometry_msgs sensor_msgs std_msgs) diff --git a/image_cb_detector/CMakeLists.txt b/image_cb_detector/CMakeLists.txt index 92448f5..2bc5c47 100644 --- a/image_cb_detector/CMakeLists.txt +++ b/image_cb_detector/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(image_cb_detector) find_package(Boost REQUIRED thread) @@ -37,7 +37,7 @@ add_library(${PROJECT_NAME} src/image_cb_detector.cpp) target_link_libraries(image_cb_detector ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS}) install(TARGETS ${PROJECT_NAME} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) @@ -49,7 +49,7 @@ target_link_libraries(image_cb_detector_action ${Boost_LIBRARIES} image_cb_detector ${OpenCV_LIBRARIES} ) -add_dependencies(image_cb_detector_action calibration_msgs_gencpp) +add_dependencies(image_cb_detector_action ${catkin_EXPORTED_TARGETS}) add_executable(rgbd_cb_detector_action src/rgbd_cb_detector_action.cpp @@ -59,7 +59,7 @@ target_link_libraries(rgbd_cb_detector_action ${Boost_LIBRARIES} image_cb_detector ${OpenCV_LIBRARIES} ) -add_dependencies(rgbd_cb_detector_action calibration_msgs_gencpp) +add_dependencies(rgbd_cb_detector_action ${catkin_EXPORTED_TARGETS}) add_executable(image_annotator src/image_annotator.cpp) target_link_libraries(image_annotator image_cb_detector ${OpenCV_LIBRARIES}) @@ -68,7 +68,7 @@ target_link_libraries(image_annotator ${Boost_LIBRARIES} image_cb_detector ${OpenCV_LIBRARIES} ) -add_dependencies(image_annotator calibration_msgs_gencpp) +add_dependencies(image_annotator ${catkin_EXPORTED_TARGETS}) install(TARGETS image_cb_detector_action rgbd_cb_detector_action image_annotator DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/interval_intersection/CMakeLists.txt b/interval_intersection/CMakeLists.txt index 73bc0ac..7383b1d 100644 --- a/interval_intersection/CMakeLists.txt +++ b/interval_intersection/CMakeLists.txt @@ -1,8 +1,8 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(interval_intersection) find_package(Boost REQUIRED thread) -find_package(catkin REQUIRED actionlib actionlib_msgs calibration_msgs geometry_msgs rosconsole roscpp roscpp_serialization rostime std_msgs) +find_package(catkin REQUIRED actionlib actionlib_msgs message_generation calibration_msgs geometry_msgs rosconsole roscpp roscpp_serialization rostime std_msgs) include_directories(${catkin_INCLUDE_DIRS}) include_directories(include) @@ -10,7 +10,7 @@ include_directories(include) # generate the messages add_action_files(DIRECTORY action FILES Config.action) -generate_messages(DEPENDENCIES actionlib_msgs geometry_msgs std_msgs) +generate_messages(DEPENDENCIES actionlib_msgs calibration_msgs geometry_msgs std_msgs) # define the package catkin_package(DEPENDS actionlib actionlib_msgs Boost calibration_msgs geometry_msgs rosconsole roscpp roscpp_serialization rostime std_msgs @@ -24,7 +24,7 @@ install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/interval_intersection/ #common commands for building c++ executables and libraries add_library(${PROJECT_NAME} src/interval_intersection.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) -add_dependencies(${PROJECT_NAME} calibration_msgs_gencpp) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) install(TARGETS ${PROJECT_NAME} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -33,7 +33,7 @@ install(TARGETS ${PROJECT_NAME} # deal with the executable add_executable(interval_intersection_node src/interval_intersection_node.cpp) target_link_libraries(interval_intersection_node ${PROJECT_NAME}) -add_dependencies(interval_intersection_node calibration_msgs_gencpp) +add_dependencies(interval_intersection_node ${catkin_EXPORTED_TARGETS}) include_directories(${BOOST_INCLUDE_DIRS}) add_executable(interval_intersection_action src/interval_intersection_action.cpp) @@ -41,7 +41,7 @@ target_link_libraries(interval_intersection_action ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PROJECT_NAME} ) -add_dependencies(interval_intersection_action ${PROJECT_NAME}_gencpp) +add_dependencies(interval_intersection_action ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) install(TARGETS interval_intersection_node interval_intersection_action DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/interval_intersection/test/CMakeLists.txt b/interval_intersection/test/CMakeLists.txt index 0f93ec5..ebbc4c7 100644 --- a/interval_intersection/test/CMakeLists.txt +++ b/interval_intersection/test/CMakeLists.txt @@ -1,3 +1,4 @@ # ********** Tests ********** catkin_add_gtest(interval_intersection_unittest interval_intersection_unittest.cpp) +add_dependencies(interval_intersection_unittest ${catkin_EXPORTED_TARGETS}) target_link_libraries(interval_intersection_unittest ${PROJECT_NAME}) diff --git a/joint_states_settler/CMakeLists.txt b/joint_states_settler/CMakeLists.txt index f1338d3..c7c766a 100644 --- a/joint_states_settler/CMakeLists.txt +++ b/joint_states_settler/CMakeLists.txt @@ -26,7 +26,7 @@ install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/joint_states_settler/ add_library(${PROJECT_NAME} src/joint_states_deflater.cpp src/joint_states_settler.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS}) install(TARGETS ${PROJECT_NAME} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) @@ -36,11 +36,11 @@ add_executable(joint_states_settler_action src/joint_states_settler_action.cpp) target_link_libraries(joint_states_settler_action ${Boost_LIBRARIES} ${PROJECT_NAME} ) -add_dependencies(joint_states_settler_action calibration_msgs_gencpp) +add_dependencies(joint_states_settler_action ${catkin_EXPORTED_TARGETS}) add_executable(view_interval src/view_interval.cpp) target_link_libraries(view_interval ${catkin_LIBRARIES}) -add_dependencies(view_interval calibration_msgs_gencpp) +add_dependencies(view_interval ${catkin_EXPORTED_TARGETS}) install(TARGETS joint_states_settler_action view_interval DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/laser_cb_detector/CMakeLists.txt b/laser_cb_detector/CMakeLists.txt index 458e0f2..5c9597c 100644 --- a/laser_cb_detector/CMakeLists.txt +++ b/laser_cb_detector/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(laser_cb_detector) find_package(Boost REQUIRED thread) @@ -31,7 +31,7 @@ add_library(${PROJECT_NAME} src/cv_laser_bridge.cpp target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS}) install(TARGETS ${PROJECT_NAME} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) @@ -50,7 +50,7 @@ target_link_libraries(laser_interval_calc_node ${Boost_LIBRARIES} ${PROJECT_NAME} ${OpenCV_LIBRARIES} ) -add_dependencies(laser_interval_calc_node calibration_msgs_gencpp) +add_dependencies(laser_interval_calc_node ${catkin_EXPORTED_TARGETS}) install(TARGETS laser_cb_detector_node laser_interval_calc_node DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/laser_cb_detector/test/CMakeLists.txt b/laser_cb_detector/test/CMakeLists.txt index 45fa595..081cf96 100644 --- a/laser_cb_detector/test/CMakeLists.txt +++ b/laser_cb_detector/test/CMakeLists.txt @@ -4,7 +4,7 @@ add_definitions(-DTEST_PATH=${CMAKE_CURRENT_SOURCE_DIR}) catkin_add_gtest(cv_laser_bridge_unittest cv_laser_bridge_unittest.cpp) target_link_libraries(cv_laser_bridge_unittest laser_cb_detector) -add_dependencies(cv_laser_bridge_unittest calibration_msgs_gencpp) +add_dependencies(cv_laser_bridge_unittest ${catkin_EXPORTED_TARGETS}) catkin_add_gtest(laser_cb_detector_unittest laser_cb_detector_unittest.cpp) target_link_libraries(laser_cb_detector_unittest laser_cb_detector) diff --git a/monocam_settler/CMakeLists.txt b/monocam_settler/CMakeLists.txt index f0c5034..a13f864 100644 --- a/monocam_settler/CMakeLists.txt +++ b/monocam_settler/CMakeLists.txt @@ -42,7 +42,7 @@ target_link_libraries(monocam_settler_action ${Boost_LIBRARIES} install(TARGETS monocam_settler_action DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -add_dependencies(monocam_settler_action calibration_msgs_gencpp) +add_dependencies(monocam_settler_action ${catkin_EXPORTED_TARGETS}) if(CATKIN_ENABLE_TESTING) add_subdirectory(test EXCLUDE_FROM_ALL) diff --git a/settlerlib/CMakeLists.txt b/settlerlib/CMakeLists.txt index 10c3c6e..4a15621 100644 --- a/settlerlib/CMakeLists.txt +++ b/settlerlib/CMakeLists.txt @@ -19,7 +19,7 @@ add_library(${PROJECT_NAME} src/interval_calc.cpp src/deflated.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) -add_dependencies(${PROJECT_NAME} calibration_msgs_gencpp) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${PROJECT_NAME} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} From a700b96108e44688cc3d7aa9d48d699f460fc423 Mon Sep 17 00:00:00 2001 From: Dave Feil-Seifer Date: Tue, 11 May 2021 14:19:45 -0700 Subject: [PATCH 09/10] fixed python3 compatibility issues --- calibration_estimation/package.xml | 1 + .../error_visualization.py | 8 +++---- .../multi_step_cov_estimator.py | 6 ++--- .../src/calibration_estimation/urdf_params.py | 4 ++-- .../test/camera_chain_sensor_unittest.py | 8 +++---- .../test/camera_unittest.py | 12 +++++----- .../test/chain_sensor_unittest.py | 16 +++++++------- .../test/checkerboard_unittest.py | 8 +++---- .../test/full_chain_unittest.py | 14 ++++++------ .../test/joint_chain_unittest.py | 10 ++++----- .../test/opt_runner_unittest.py | 22 +++++++++---------- .../test/single_transform_unittest.py | 20 ++++++++--------- .../test/tilting_laser_sensor_unittest.py | 18 +++++++-------- .../test/tilting_laser_unittest.py | 22 +++++++++---------- calibration_estimation/test/urdf_unittest.py | 4 ++-- calibration_launch/scripts/capture_exec.py | 16 +++++++------- 16 files changed, 95 insertions(+), 94 deletions(-) diff --git a/calibration_estimation/package.xml b/calibration_estimation/package.xml index 640c613..f92d28e 100644 --- a/calibration_estimation/package.xml +++ b/calibration_estimation/package.xml @@ -12,6 +12,7 @@ http://ros.org/wiki/calibration_estimation catkin + calibration_msgs calibration_msgs python-matplotlib diff --git a/calibration_estimation/src/calibration_estimation/error_visualization.py b/calibration_estimation/src/calibration_estimation/error_visualization.py index 318d460..7b2b79b 100755 --- a/calibration_estimation/src/calibration_estimation/error_visualization.py +++ b/calibration_estimation/src/calibration_estimation/error_visualization.py @@ -77,7 +77,7 @@ def usage(): rospy.logerr("Bagfile does not exist. Exiting...") sys.exit(1) - loop_list = yaml.load(open(loop_list_filename)) + loop_list = yaml.safe_load(open(loop_list_filename)) robot_description = get_robot_description(bag_filename) config_param_name = "calibration_config" @@ -100,10 +100,10 @@ def usage(): cur_step = step_list[-1] # Load the resulting system definition - system_def_dict = yaml.load(open(output_dir + "/" + cur_step["output_filename"] + ".yaml")) + system_def_dict = yaml.safe_load(open(output_dir + "/" + cur_step["output_filename"] + ".yaml")) system_def = UrdfParams(robot_description, system_def_dict) - cb_poses = yaml.load(open(output_dir + "/" + cur_step["output_filename"] + "_poses.yaml")) - free_dict = yaml.load(cur_step["free_params"]) + cb_poses = yaml.safe_load(open(output_dir + "/" + cur_step["output_filename"] + "_poses.yaml")) + free_dict = yaml.safe_load(cur_step["free_params"]) sample_skip_list = rospy.get_param('calibration_skip_list', []) scatter_list = [] diff --git a/calibration_estimation/src/calibration_estimation/multi_step_cov_estimator.py b/calibration_estimation/src/calibration_estimation/multi_step_cov_estimator.py index 3a1ba91..f970b3b 100755 --- a/calibration_estimation/src/calibration_estimation/multi_step_cov_estimator.py +++ b/calibration_estimation/src/calibration_estimation/multi_step_cov_estimator.py @@ -261,7 +261,7 @@ def update_urdf(urdf, calibrated_params): if sensors_name not in config.keys(): rospy.logerr("Could not find namespace [%s/%s]. Please populate this namespace with sensors.", (config_param_name, sensors_name)) sys.exit(1) - #sensors_dump = [yaml.load(x) for x in config[sensors_name].values()] + #sensors_dump = [yaml.safe_load(x) for x in config[sensors_name].values()] all_sensors_dict = build_sensor_defs(config[sensors_name]) all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()])) @@ -275,7 +275,7 @@ def update_urdf(urdf, calibrated_params): if 'initial_poses' in config.keys(): if os.path.exists(config['initial_poses']): with open(config['initial_poses']) as f: - previous_pose_guesses = numpy.array(yaml.load(f)) + previous_pose_guesses = numpy.array(yaml.safe_load(f)) else: rospy.logwarn("cannot find %s" % (config['initial_poses'])) previous_pose_guesses = numpy.zeros([msg_count,6]) @@ -348,7 +348,7 @@ def update_urdf(urdf, calibrated_params): output_dict = robot_params.params_to_config(robot_params.deflate()) output_poses = previous_pose_guesses else: - free_dict = yaml.load(cur_step["free_params"]) + free_dict = yaml.safe_load(cur_step["free_params"]) use_cov = cur_step['use_cov'] if use_cov: print "Executing step with covariance calculations" diff --git a/calibration_estimation/src/calibration_estimation/urdf_params.py b/calibration_estimation/src/calibration_estimation/urdf_params.py index 7114f5a..63b71f5 100755 --- a/calibration_estimation/src/calibration_estimation/urdf_params.py +++ b/calibration_estimation/src/calibration_estimation/urdf_params.py @@ -173,13 +173,13 @@ def configure(self, urdf, config_dict): # we can handle limited rotations here rot = urdf.joint_map[joint_name].origin.rotation if rot != None and (sum([abs(x) for x in rot]) - rot[abs(this_config["axis"][-1])-4]) > 0.001: - print 'Joint origin is rotated, calibration will fail: ', joint_name + print ('Joint origin is rotated, calibration will fail: ', joint_name) elif urdf.joint_map[joint_name].joint_type == 'prismatic': this_config["active_joints"].append(joint_name) axis = urdf.joint_map[joint_name].axis this_config["axis"].append( sum( [i[0]*int(i[1]) for i in zip([1,2,3], axis)] ) ) elif urdf.joint_map[joint_name].joint_type != 'fixed': - print 'Unknown joint type:', urdf.joint_map[joint_name].joint_type + print ('Unknown joint type:', urdf.joint_map[joint_name].joint_type) # put a checkerboard in it's hand self.urdf.add_link(Link(chain_name+"_cb_link")) self.urdf.add_joint(Joint(chain_name+"_cb",this_config['tip'],chain_name+"_cb_link","fixed",origin=Pose([0.0,0.0,0.0],[0.0,0.0,0.0]))) diff --git a/calibration_estimation/test/camera_chain_sensor_unittest.py b/calibration_estimation/test/camera_chain_sensor_unittest.py index 6474608..e53c9bd 100755 --- a/calibration_estimation/test/camera_chain_sensor_unittest.py +++ b/calibration_estimation/test/camera_chain_sensor_unittest.py @@ -69,7 +69,7 @@ def loadConfigList(): link_num: 1 after_chain: [transformB] ''' - config_dict = yaml.load(config_yaml) + config_dict = yaml.safe_load(config_yaml) return config_dict @@ -111,7 +111,7 @@ def test_basic_no_match(self): class TestCameraChainSensor(unittest.TestCase): def load(self): - config = yaml.load(''' + config = yaml.safe_load(''' camera_id: camA chain: before_chain: [transformA] @@ -121,7 +121,7 @@ def load(self): ''') robot_params = RobotParams() - robot_params.configure( yaml.load(''' + robot_params.configure( yaml.safe_load(''' chains: chainA: transforms: @@ -179,7 +179,7 @@ def test_cov(self): [1, 1]]) cov = sensor.compute_cov(target) - print "\ncov:\n", cov + print ("\ncov:\n", cov) self.assertAlmostEqual(cov[0,0], 1.0, 6) self.assertAlmostEqual(cov[1,0], 0.0, 6) diff --git a/calibration_estimation/test/camera_unittest.py b/calibration_estimation/test/camera_unittest.py index 06f29d0..13f6969 100755 --- a/calibration_estimation/test/camera_unittest.py +++ b/calibration_estimation/test/camera_unittest.py @@ -70,8 +70,8 @@ def test_project_easy_1(self): result = cam.project(P_list, pts) - print "" - print result + print ("") + print (result) self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6) @@ -91,8 +91,8 @@ def test_project_easy_2(self): result = cam.project(P_list, pts) - print "" - print result + print ("") + print (result) self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6) @@ -114,8 +114,8 @@ def test_project_easy_3(self): result = cam.project(P_list, pts) - print "" - print result + print ("") + print (result) self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6) diff --git a/calibration_estimation/test/chain_sensor_unittest.py b/calibration_estimation/test/chain_sensor_unittest.py index 55af730..0a05cd9 100755 --- a/calibration_estimation/test/chain_sensor_unittest.py +++ b/calibration_estimation/test/chain_sensor_unittest.py @@ -89,7 +89,7 @@ def loadSystem(): ''' - config = yaml.load(''' + config = yaml.safe_load(''' sensors: chains: chainA: @@ -162,7 +162,7 @@ def test_cov(self): "boardA") block.update_config(robot_params) cov = block.compute_cov(None) - print cov + print (cov) #self.assertAlmostEqual(cov[0,0], 0.0, 6) #self.assertAlmostEqual(cov[1,0], 0.0, 6) @@ -188,8 +188,8 @@ def test_update1(self): self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6) - print "z=\n",z - print "target=\n",target + print ("z=\n",z) + print ("target=\n",target) self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6) self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6) @@ -213,8 +213,8 @@ def test_update2(self): self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6) - print "z=\n",z - print "target=\n",target + print ("z=\n",z) + print ("target=\n",target) self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6) self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6) @@ -238,8 +238,8 @@ def test_update3(self): self.assertAlmostEqual(numpy.linalg.norm(target-h), 0.0, 6) - print "z=\n",z - print "target=\n",target + print ("z=\n",z) + print ("target=\n",target) self.assertAlmostEqual(numpy.linalg.norm(target-z), 0.0, 6) self.assertAlmostEqual(numpy.linalg.norm(r - numpy.zeros([12])), 0.0, 6) diff --git a/calibration_estimation/test/checkerboard_unittest.py b/calibration_estimation/test/checkerboard_unittest.py index 7c2fe21..7885d06 100755 --- a/calibration_estimation/test/checkerboard_unittest.py +++ b/calibration_estimation/test/checkerboard_unittest.py @@ -70,8 +70,8 @@ def test_deflate(self): param_vec = matrix([10,20], float).T cb.inflate( param_vec ) result = cb.deflate() - print "" - print result + print ("") + print (result) self.assertAlmostEqual(numpy.linalg.norm(result - param_vec), 0.0, 6) def test_generate_points(self): @@ -84,8 +84,8 @@ def test_generate_points(self): [ 20, 20, 0, 0,-20,-20], [ 0, 0, 0, 0, 0, 0], [ 1, 1, 1, 1, 1, 1] ], float) - print "" - print result + print ("") + print (result) self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6) def test_get_length(self): diff --git a/calibration_estimation/test/full_chain_unittest.py b/calibration_estimation/test/full_chain_unittest.py index 2081a93..27efd90 100755 --- a/calibration_estimation/test/full_chain_unittest.py +++ b/calibration_estimation/test/full_chain_unittest.py @@ -80,7 +80,7 @@ def loadSystem1(): ''' - config = yaml.load(''' + config = yaml.safe_load(''' sensors: chains: chain1: @@ -100,7 +100,7 @@ def loadSystem1(): class TestFullChainCalcBlock(unittest.TestCase): def test_fk_1(self): - print "" + print ("") params = loadSystem1() chain = FullChainRobotParams('chain1', 'j3_link') chain.update_config(params) @@ -111,11 +111,11 @@ def test_fk_1(self): [ 0, 1, 0, 0], [ 0, 0, 1,20], [ 0, 0, 0, 1]], float ) - print result + print (result) self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6) def test_fk_2(self): - print "" + print ("") params = loadSystem1() chain = FullChainRobotParams('chain1', 'j3_link') chain.update_config(params) @@ -126,11 +126,11 @@ def test_fk_2(self): [ 1, 0, 0, 0], [ 0, 0, 1,20], [ 0, 0, 0, 1]], float ) - print result + print (result) self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6) def test_fk_partial(self): - print "" + print ("") params = loadSystem1() chain = FullChainRobotParams('chain1', 'j1_link') chain.update_config(params) @@ -141,7 +141,7 @@ def test_fk_partial(self): [ 0, 1, 0, 0], [ 0, 0, 1, 0], [ 0, 0, 0, 1]], float ) - print result + print (result) self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6) diff --git a/calibration_estimation/test/joint_chain_unittest.py b/calibration_estimation/test/joint_chain_unittest.py index 1f68a87..48f6726 100755 --- a/calibration_estimation/test/joint_chain_unittest.py +++ b/calibration_estimation/test/joint_chain_unittest.py @@ -48,7 +48,7 @@ class LoadJointChain(unittest.TestCase): def setUp(self): - print "" + print ("") config = ''' root: x tip: y @@ -59,7 +59,7 @@ def setUp(self): cov: joint_angles: [1, 1, 1] ''' - config_dict = yaml.load(config) + config_dict = yaml.safe_load(config) config_dict['transforms'] = { 'j1': SingleTransform([1, 0, 0, 0, 0, 0]), 'j2': SingleTransform([1, 0, 0, 0, 0, 0]), 'j3': SingleTransform([1, 0, 2, 0, 0, 0]) } @@ -115,7 +115,7 @@ def test_fk_easy2(self): chain_state = JointState() chain_state.position = [numpy.pi/2, 0, 0] eef = self.chain.fk(chain_state, 0) - print eef + print (eef) eef_expected = numpy.matrix( [[ 0,-1, 0, 1], [ 1, 0, 0, 0], [ 0, 0, 1, 0], @@ -126,7 +126,7 @@ def test_fk_easy3(self): chain_state = JointState() chain_state.position = [numpy.pi/2, numpy.pi/2, 0] eef = self.chain.fk(chain_state, 1) - print eef + print (eef) eef_expected = numpy.matrix( [[-1, 0, 0, 1], [ 0,-1, 0, 1], [ 0, 0, 1, 0], @@ -137,7 +137,7 @@ def test_fk_easy4(self): chain_state = JointState() chain_state.position = [0, 0, 0] eef = self.chain.fk(chain_state, -1) - print eef + print (eef) eef_expected = numpy.matrix( [[ 1, 0, 0, 3], [ 0, 1, 0, 0], [ 0, 0, 1, 2], diff --git a/calibration_estimation/test/opt_runner_unittest.py b/calibration_estimation/test/opt_runner_unittest.py index 67a4518..3be9066 100755 --- a/calibration_estimation/test/opt_runner_unittest.py +++ b/calibration_estimation/test/opt_runner_unittest.py @@ -53,7 +53,7 @@ class TestParamJacobian(unittest.TestCase): def test_single_sensor(self): - config = yaml.load(''' + config = yaml.safe_load(''' chain_id: chainA before_chain: [transformA] dh_link_num: 1 @@ -61,7 +61,7 @@ def test_single_sensor(self): ''') robot_params = RobotParams() - robot_params.configure( yaml.load(''' + robot_params.configure( yaml.safe_load(''' dh_chains: chainA: - [ 0, 0, 1, 0 ] @@ -80,7 +80,7 @@ def test_single_sensor(self): spacing_y: 1 ''' ) ) - free_dict = yaml.load(''' + free_dict = yaml.safe_load(''' dh_chains: chainA: - [ 1, 0, 1, 0 ] @@ -120,14 +120,14 @@ def test_single_sensor(self): class TestPoseJacobian(unittest.TestCase): def test_multisensor_pose_jacobian(self): - configA = yaml.load(''' + configA = yaml.safe_load(''' chain_id: chainA before_chain: [transformA] dh_link_num: 1 after_chain: [transformB] ''') - configB = yaml.load(''' + configB = yaml.safe_load(''' chain_id: chainB before_chain: [transformA] dh_link_num: 1 @@ -135,7 +135,7 @@ def test_multisensor_pose_jacobian(self): ''') robot_params = RobotParams() - robot_params.configure( yaml.load(''' + robot_params.configure( yaml.safe_load(''' dh_chains: chainA: - [ 0, 0, 1, 0 ] @@ -154,7 +154,7 @@ def test_multisensor_pose_jacobian(self): spacing_y: 1 ''' ) ) - free_dict = yaml.load(''' + free_dict = yaml.safe_load(''' dh_chains: chainA: - [ 1, 0, 0, 0 ] @@ -193,14 +193,14 @@ def test_multisensor_pose_jacobian(self): class TestFullJacobian(unittest.TestCase): def test_full_jacobian(self): - configA = yaml.load(''' + configA = yaml.safe_load(''' chain_id: chainA before_chain: [transformA] dh_link_num: 1 after_chain: [] ''') - configB = yaml.load(''' + configB = yaml.safe_load(''' chain_id: chainB before_chain: [] dh_link_num: 1 @@ -208,7 +208,7 @@ def test_full_jacobian(self): ''') robot_params = RobotParams() - robot_params.configure( yaml.load(''' + robot_params.configure( yaml.safe_load(''' dh_chains: chainA: - [ 0, 0, 1, 0 ] @@ -235,7 +235,7 @@ def test_full_jacobian(self): spacing_y: 1 ''' ) ) - free_dict = yaml.load(''' + free_dict = yaml.safe_load(''' dh_chains: chainA: - [ 1, 0, 0, 0 ] diff --git a/calibration_estimation/test/single_transform_unittest.py b/calibration_estimation/test/single_transform_unittest.py index 9cab617..3773270 100755 --- a/calibration_estimation/test/single_transform_unittest.py +++ b/calibration_estimation/test/single_transform_unittest.py @@ -64,8 +64,8 @@ def test_inflate(self): [ 0, 0, 1, 0], [ 0, 0, 0, 1]], float ) - print "" - print st.transform + print ("") + print (st.transform) self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6) @@ -91,8 +91,8 @@ def test_easy_trans_1(self): [ 0, 0, 1, 3], [ 0, 0, 0, 1]], float ) - print "" - print st.transform + print ("") + print (st.transform) self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6) @@ -102,8 +102,8 @@ def test_easy_rot_1(self): [ 1, 0, 0, 0], [ 0, 0, 1, 0], [ 0, 0, 0, 1]], float ) - print "" - print st.transform + print ("") + print (st.transform) self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6) @@ -113,8 +113,8 @@ def test_easy_rot_2(self): [ 0, 1, 0, 0], [-1, 0, 0, 0], [ 0, 0, 0, 1]], float ) - print "" - print st.transform + print ("") + print (st.transform) self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6) @@ -124,8 +124,8 @@ def test_easy_rot_3(self): [ 0, 0,-1, 0], [ 0, 1, 0, 0], [ 0, 0, 0, 1]], float ) - print "" - print st.transform + print ("") + print (st.transform) self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6) diff --git a/calibration_estimation/test/tilting_laser_sensor_unittest.py b/calibration_estimation/test/tilting_laser_sensor_unittest.py index beebad6..062bfa9 100755 --- a/calibration_estimation/test/tilting_laser_sensor_unittest.py +++ b/calibration_estimation/test/tilting_laser_sensor_unittest.py @@ -55,7 +55,7 @@ def loadConfigList(): - laser_id: laser1 sensor_id: laser1 ''' - config_dict = yaml.load(config_yaml) + config_dict = yaml.safe_load(config_yaml) return config_dict class TestTiltingLaserBundler(unittest.TestCase): @@ -97,7 +97,7 @@ def loadSystem(): ''' - config = yaml.load(''' + config = yaml.safe_load(''' sensors: chains: {} rectified_cams: {} @@ -121,7 +121,7 @@ def loadSystem(): class TestTiltingLaser(unittest.TestCase): def test_cov(self): - print "" + print ("") config, robot_params = loadSystem() joint_points = [ JointState(position=[0,0,1]), @@ -134,8 +134,8 @@ def test_cov(self): cov = sensor.compute_cov(None) - print "Cov:" - print cov + print ("Cov:") + print (cov) self.assertAlmostEqual(cov[0,0], 1.0, 6) self.assertAlmostEqual(cov[1,1], 1.0, 6) @@ -145,7 +145,7 @@ def test_cov(self): self.assertAlmostEqual(cov[5,5], 1.0, 6) def test_gamma(self): - print "" + print ("") config, robot_params = loadSystem() joint_points = [ JointState(position=[0,0,1]), @@ -157,8 +157,8 @@ def test_gamma(self): sensor.update_config(robot_params) gamma = sensor.compute_marginal_gamma_sqrt(None) - print "Gamma:" - print gamma + print ("Gamma:") + print (gamma) self.assertAlmostEqual(gamma[0,0], 1.0, 6) self.assertAlmostEqual(gamma[1,1], 1.0, 6) self.assertAlmostEqual(gamma[2,2], 1.0, 6) @@ -167,7 +167,7 @@ def test_gamma(self): self.assertAlmostEqual(gamma[5,5], 1.0, 6) def test_tilting_laser_1(self): - print "" + print ("") config, robot_params = loadSystem() joint_points = [ JointState(position=[0,0,0]), diff --git a/calibration_estimation/test/tilting_laser_unittest.py b/calibration_estimation/test/tilting_laser_unittest.py index 99f8e08..c18d602 100755 --- a/calibration_estimation/test/tilting_laser_unittest.py +++ b/calibration_estimation/test/tilting_laser_unittest.py @@ -70,7 +70,7 @@ def loadSystem1(): ''' - config = yaml.load(''' + config = yaml.safe_load(''' sensors: chains: {} rectified_cams: {} @@ -97,8 +97,8 @@ def test_project_point_easy_1(self): tl.update_config(params) result = tl.project_point_to_3D([0, 0, 0]) expected = numpy.matrix( [20, 0, 10, 1] ).T - print "" - print result + print ("") + print (result) self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6) def test_project_point_easy_2(self): @@ -107,8 +107,8 @@ def test_project_point_easy_2(self): tl.update_config(params) result = tl.project_point_to_3D([0, pi/2, 1]) expected = numpy.matrix( [20, 1, 10, 1] ).T - print "" - print result + print ("") + print (result) self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6) def test_project_point_easy_3(self): @@ -117,8 +117,8 @@ def test_project_point_easy_3(self): tl.update_config(params) result = tl.project_point_to_3D([pi/2, 0, 0]) expected = numpy.matrix( [0 , 0,-10, 1] ).T - print "" - print result + print ("") + print (result) self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6) def test_project_point_easy_4(self): @@ -127,8 +127,8 @@ def test_project_point_easy_4(self): tl.update_config(params) result = tl.project_point_to_3D([pi/2, -pi/2, 15]) expected = numpy.matrix( [0 ,-15, -10, 1] ).T - print "" - print result + print ("") + print (result) self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6) def test_project(self): @@ -146,8 +146,8 @@ def test_project(self): [ 10, 10, -10, -10 ], [ 1, 1, 1, 1 ] ] ) - print - print result + print () + print (result) self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6) diff --git a/calibration_estimation/test/urdf_unittest.py b/calibration_estimation/test/urdf_unittest.py index 5fe0b13..c85d5f8 100755 --- a/calibration_estimation/test/urdf_unittest.py +++ b/calibration_estimation/test/urdf_unittest.py @@ -51,7 +51,7 @@ def loadSystem(): outfile = open(raw_xml, 'w') outfile.write(robot_description) outfile.close() - config = yaml.load(''' + config = yaml.safe_load(''' sensors: chains: {} rectified_cams: {} @@ -63,7 +63,7 @@ def loadSystem(): class TestUrdfWriter(unittest.TestCase): def test_write(self): - print "" + print ("") params = loadSystem() outfile = open(calibrated_xml, 'w') outfile.write( params.urdf.to_xml() ) diff --git a/calibration_launch/scripts/capture_exec.py b/calibration_launch/scripts/capture_exec.py index 9dafc9d..7772c07 100755 --- a/calibration_launch/scripts/capture_exec.py +++ b/calibration_launch/scripts/capture_exec.py @@ -53,10 +53,10 @@ class CaptureExecutive: def __init__(self, config_dir, system, robot_desc, output_debug=False): # Load configs - self.cam_config = yaml.load(open(config_dir + "/cam_config.yaml")) - self.chain_config = yaml.load(open(config_dir + "/chain_config.yaml")) - self.laser_config = yaml.load(open(config_dir + "/laser_config.yaml")) - self.controller_config = yaml.load(open(config_dir + "/controller_config.yaml")) + self.cam_config = yaml.safe_load(open(config_dir + "/cam_config.yaml")) + self.chain_config = yaml.safe_load(open(config_dir + "/chain_config.yaml")) + self.laser_config = yaml.safe_load(open(config_dir + "/laser_config.yaml")) + self.controller_config = yaml.safe_load(open(config_dir + "/controller_config.yaml")) # Not all robots have lasers.... :( if self.laser_config == None: self.laser_config = dict() @@ -73,7 +73,7 @@ def __init__(self, config_dir, system, robot_desc, output_debug=False): links = URDF().parse(robot_desc).link_map.keys() # load system config - system = yaml.load(open(system)) + system = yaml.safe_load(open(system)) # remove cams that are not on urdf for cam in self.cam_config.keys(): @@ -329,7 +329,7 @@ def add_laser_measurement(self, laser_id, msg, interval_start, interval_end): sample_failure = dict() for directory in os.listdir(samples_dir): try: - sample_options[directory] = yaml.load(open(samples_dir + '/' + directory + '/config.yaml')) + sample_options[directory] = yaml.safe_load(open(samples_dir + '/' + directory + '/config.yaml')) sample_steps.append(directory) except IOError: continue @@ -352,7 +352,7 @@ def add_laser_measurement(self, laser_id, msg, interval_start, interval_end): rospy.logfatal(samples_dir + '/' + step + '/' + ' is not found' ) rospy.logfatal('please run make_samples.py' ) sys.exit(-1) - cur_config = yaml.load(open(full_paths[0])) + cur_config = yaml.safe_load(open(full_paths[0])) m_robot = executive.capture(cur_config, rospy.Duration(0.01)) while not rospy.is_shutdown() and keep_collecting: print @@ -365,7 +365,7 @@ def add_laser_measurement(self, laser_id, msg, interval_start, interval_end): else: for cur_sample_path in full_paths: print "On %s sample [%s]" % (sample_options[step]["group"], cur_sample_path) - cur_config = yaml.load(open(cur_sample_path)) + cur_config = yaml.safe_load(open(cur_sample_path)) m_robot = executive.capture(cur_config, rospy.Duration(40)) if m_robot is None: print "--------------- Failed To Capture a %s Sample -----------------" % sample_options[step]["group"] From 4201ef3212e2cf5e76bee0cff032625f77b34a14 Mon Sep 17 00:00:00 2001 From: Dave Feil-Seifer Date: Tue, 11 May 2021 14:38:04 -0700 Subject: [PATCH 10/10] fixed test errors for python3 compatibility --- .../src/calibration_estimation/full_chain.py | 6 +++--- .../src/calibration_estimation/sensors/chain_sensor.py | 3 ++- .../calibration_estimation/sensors/tilting_laser_sensor.py | 3 ++- calibration_estimation/test/chain_sensor_unittest.py | 2 +- calibration_estimation/test/full_chain_unittest.py | 2 +- .../test/tilting_laser_sensor_unittest.py | 2 +- calibration_estimation/test/tilting_laser_unittest.py | 2 +- 7 files changed, 11 insertions(+), 9 deletions(-) diff --git a/calibration_estimation/src/calibration_estimation/full_chain.py b/calibration_estimation/src/calibration_estimation/full_chain.py index bd5e4c9..ba98102 100644 --- a/calibration_estimation/src/calibration_estimation/full_chain.py +++ b/calibration_estimation/src/calibration_estimation/full_chain.py @@ -88,9 +88,9 @@ def build_sparsity_dict(self): sparsity['transforms'] = {} sparsity['chains'] = {} if self.chain_id is not None: - for cur_transform in ( self.calc_block._before_chain_Ts + \ - self.calc_block._chain._transforms.values() + \ - self.calc_block._after_chain_Ts ): + for cur_transform in ( list(self.calc_block._before_chain_Ts) + \ + list(self.calc_block._chain._transforms.values()) + \ + list(self.calc_block._after_chain_Ts) ): sparsity['transforms'][cur_transform._name] = [1, 1, 1, 1, 1, 1] sparsity['chains'][self.chain_id] = {} link_num = self.calc_block._link_num diff --git a/calibration_estimation/src/calibration_estimation/sensors/chain_sensor.py b/calibration_estimation/src/calibration_estimation/sensors/chain_sensor.py index 004f83a..f9bdcfc 100644 --- a/calibration_estimation/src/calibration_estimation/sensors/chain_sensor.py +++ b/calibration_estimation/src/calibration_estimation/sensors/chain_sensor.py @@ -135,7 +135,8 @@ def compute_cov(self, target_pts): cov_angles = [x*x for x in self._full_chain.calc_block._chain._cov_dict['joint_angles']] cov = matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt) - if ( self._full_chain.calc_block._chain._cov_dict.has_key('translation') ): + #if ( self._full_chain.calc_block._chain._cov_dict.has_key('translation') ): + if ( 'translation' in self._full_chain.calc_block._chain._cov_dict ): translation_var = self._full_chain.calc_block._chain._cov_dict['translation']; translation_cov = numpy.diag(translation_var*(self.get_residual_length()/3)) cov = cov + translation_cov diff --git a/calibration_estimation/src/calibration_estimation/sensors/tilting_laser_sensor.py b/calibration_estimation/src/calibration_estimation/sensors/tilting_laser_sensor.py index d2437b9..770ed70 100644 --- a/calibration_estimation/src/calibration_estimation/sensors/tilting_laser_sensor.py +++ b/calibration_estimation/src/calibration_estimation/sensors/tilting_laser_sensor.py @@ -47,6 +47,7 @@ import roslib; roslib.load_manifest('calibration_estimation') import rospy import numpy +import math class TiltingLaserBundler: def __init__(self, valid_configs): @@ -97,7 +98,7 @@ def compute_marginal_gamma_sqrt(self, target_pts): # ----- Populate Here ----- cov = self.compute_cov(target_pts) gamma = matrix(zeros(cov.shape)) - num_pts = self.get_residual_length()/3 + num_pts = math.floor(self.get_residual_length()/3) for k in range(num_pts): first = 3*k diff --git a/calibration_estimation/test/chain_sensor_unittest.py b/calibration_estimation/test/chain_sensor_unittest.py index 0a05cd9..2269957 100755 --- a/calibration_estimation/test/chain_sensor_unittest.py +++ b/calibration_estimation/test/chain_sensor_unittest.py @@ -59,7 +59,7 @@ def loadSystem(): urdf = ''' - + diff --git a/calibration_estimation/test/full_chain_unittest.py b/calibration_estimation/test/full_chain_unittest.py index 27efd90..7aa8ad5 100755 --- a/calibration_estimation/test/full_chain_unittest.py +++ b/calibration_estimation/test/full_chain_unittest.py @@ -50,7 +50,7 @@ def loadSystem1(): urdf = ''' - + diff --git a/calibration_estimation/test/tilting_laser_sensor_unittest.py b/calibration_estimation/test/tilting_laser_sensor_unittest.py index 062bfa9..a0ebd4f 100755 --- a/calibration_estimation/test/tilting_laser_sensor_unittest.py +++ b/calibration_estimation/test/tilting_laser_sensor_unittest.py @@ -74,7 +74,7 @@ def test_basic(self): def loadSystem(): urdf = ''' - + diff --git a/calibration_estimation/test/tilting_laser_unittest.py b/calibration_estimation/test/tilting_laser_unittest.py index c18d602..1441e68 100755 --- a/calibration_estimation/test/tilting_laser_unittest.py +++ b/calibration_estimation/test/tilting_laser_unittest.py @@ -47,7 +47,7 @@ def loadSystem1(): urdf = ''' - +