diff --git a/rct_ros_tools/include/rct_ros_tools/parameter_loaders.h b/rct_ros_tools/include/rct_ros_tools/parameter_loaders.h index e2b31f61..f68698be 100644 --- a/rct_ros_tools/include/rct_ros_tools/parameter_loaders.h +++ b/rct_ros_tools/include/rct_ros_tools/parameter_loaders.h @@ -1,6 +1,8 @@ #ifndef RCT_PARAMETER_LOADERS_H #define RCT_PARAMETER_LOADERS_H +#include + #include #include #include @@ -8,12 +10,79 @@ namespace rct_ros_tools { +/** + * \brief Thrown during errors in loading or parsing data files + */ +class BadFileException : public std::runtime_error +{ +public: + BadFileException(const std::string& what) : std::runtime_error(what) {} +}; + +/** + * \brief Load a target from a ROS parameter + * \throws ros::InvalidParameterException + */ +rct_image_tools::ModifiedCircleGridTarget loadTarget(const ros::NodeHandle& nh, const std::string& key); + +/** + * \brief Load a target from a ROS parameter. Returns false if an error occurs. + */ bool loadTarget(const ros::NodeHandle& nh, const std::string& key, rct_image_tools::ModifiedCircleGridTarget& target); + +/** + * \brief Load a target from a YAML file + * \throws rct_ros_tools::BadFileException + */ +rct_image_tools::ModifiedCircleGridTarget loadTarget(const std::string& path); + +/** + * \brief Load a target from a YAML file. Returns false if an error occurs. + */ bool loadTarget(const std::string& path, rct_image_tools::ModifiedCircleGridTarget& target); +/** + * \brief Load camera intrinsics from a ROS parameter + * \throws ros::InvalidParameterException + */ +rct_optimizations::CameraIntrinsics loadIntrinsics(const ros::NodeHandle& nh, const std::string& key); + +/** + * \brief Load a target from a ROS parameter. Returns false if an error occurs. + */ bool loadIntrinsics(const ros::NodeHandle& nh, const std::string& key, rct_optimizations::CameraIntrinsics& intr); +/** + * \brief Load camera intrinsics from a YAML file + * \throws rct_ros_tools::BadFileException + */ +rct_optimizations::CameraIntrinsics loadIntrinsics(const std::string& path); + +/** + * \brief Load camera intrinsics from a YAML file. Returns false if an error occurs. + */ +bool loadIntrinsics(const std::string& path, rct_optimizations::CameraIntrinsics& intrinsics); + +/** + * \brief Load a pose from a ROS parameter + * \throws ros::InvalidParameterException + */ +Eigen::Isometry3d loadPose(const ros::NodeHandle& nh, const std::string& key); + +/** + * \brief Load a pose from a ROS parameter. Returns false if an error occurs. + */ bool loadPose(const ros::NodeHandle& nh, const std::string& key, Eigen::Isometry3d& pose); + +/** + * \brief Load a pose from a YAML file + * \throws rct_ros_tools::BadFileException + */ +Eigen::Isometry3d loadPose(const std::string& path); + +/** + * \brief Load a pose from a YAML file. Returns false if an error occurs. + */ bool loadPose(const std::string& path, Eigen::Isometry3d& pose); } diff --git a/rct_ros_tools/src/data_loader/parameter_loaders.cpp b/rct_ros_tools/src/data_loader/parameter_loaders.cpp index 8b8e93f3..8fa6ccbc 100644 --- a/rct_ros_tools/src/data_loader/parameter_loaders.cpp +++ b/rct_ros_tools/src/data_loader/parameter_loaders.cpp @@ -15,90 +15,199 @@ static bool read(XmlRpc::XmlRpcValue& xml, const std::string& key, T& value) return true; } -bool rct_ros_tools::loadTarget(const ros::NodeHandle& nh, const std::string& key, - rct_image_tools::ModifiedCircleGridTarget& target) +rct_image_tools::ModifiedCircleGridTarget rct_ros_tools::loadTarget(const ros::NodeHandle& nh, const std::string& key) { XmlRpc::XmlRpcValue xml; - if (!nh.getParam(key, xml)) return false; + if (!nh.getParam(key, xml)) throw ros::InvalidParameterException(key); int rows = 0; int cols = 0; double spacing = 0.0; - if (!read(xml, "rows", rows)) return false; - if (!read(xml, "cols", cols)) return false; - if (!read(xml, "spacing", spacing)) return false; + if (!read(xml, "rows", rows)) throw ros::InvalidParameterException(key + "/rows"); + if (!read(xml, "cols", cols)) throw ros::InvalidParameterException(key + "/cols"); + if (!read(xml, "spacing", spacing)) throw ros::InvalidParameterException(key + "/spacing"); + + return rct_image_tools::ModifiedCircleGridTarget(rows, cols, spacing); +} - target = rct_image_tools::ModifiedCircleGridTarget(rows, cols, spacing); +bool rct_ros_tools::loadTarget(const ros::NodeHandle& nh, const std::string& key, + rct_image_tools::ModifiedCircleGridTarget& target) +{ + try + { + target = loadTarget(nh, key); + } + catch (ros::InvalidParameterException &ex) + { + ROS_ERROR_STREAM("Failed to load target parameter: " << ex.what()); + return false; + } return true; } -bool rct_ros_tools::loadTarget(const std::string& path, rct_image_tools::ModifiedCircleGridTarget& target) +rct_image_tools::ModifiedCircleGridTarget rct_ros_tools::loadTarget(const std::string &path) { - YAML::Node n = YAML::LoadFile(path); - int rows = n["target_definition"]["rows"].as(); - int cols = n["target_definition"]["cols"].as(); - double spacing = n["target_definition"]["spacing"].as(); // (meters between dot centers) + try + { + YAML::Node n = YAML::LoadFile(path); + int rows = n["target_definition"]["rows"].as(); + int cols = n["target_definition"]["cols"].as(); + double spacing = n["target_definition"]["spacing"].as(); // (meters between dot centers) + return rct_image_tools::ModifiedCircleGridTarget(rows, cols, spacing); + } + catch (YAML::Exception &ex) + { + throw BadFileException(std::string("YAML failure: ") + ex.what()); + } +} - target = rct_image_tools::ModifiedCircleGridTarget(rows, cols, spacing); +bool rct_ros_tools::loadTarget(const std::string& path, rct_image_tools::ModifiedCircleGridTarget& target) +{ + try + { + target = loadTarget(path); + } + catch (ros::InvalidParameterException &ex) + { + ROS_ERROR_STREAM("Failed to load target from file: " << ex.what()); + return false; + } return true; } +rct_optimizations::CameraIntrinsics rct_ros_tools::loadIntrinsics(const ros::NodeHandle &nh, const std::string &key) +{ + XmlRpc::XmlRpcValue xml; + if (!nh.getParam(key, xml)) throw ros::InvalidParameterException(key); + + rct_optimizations::CameraIntrinsics intr; + if (!read(xml, "fx", intr.fx())) throw ros::InvalidParameterException(key + "/fx"); + if (!read(xml, "fy", intr.fy())) throw ros::InvalidParameterException(key + "/fy"); + if (!read(xml, "cx", intr.cx())) throw ros::InvalidParameterException(key + "/cx"); + if (!read(xml, "cy", intr.cy())) throw ros::InvalidParameterException(key + "/cy"); + + return intr; +} + bool rct_ros_tools::loadIntrinsics(const ros::NodeHandle& nh, const std::string& key, rct_optimizations::CameraIntrinsics& intr) { - XmlRpc::XmlRpcValue xml; - if (!nh.getParam(key, xml)) return false; + try + { + intr = loadIntrinsics(nh, key); + } + catch (ros::InvalidParameterException &ex) + { + ROS_ERROR_STREAM("Failed to load intrisic parameter: " << ex.what()); + return false; + } + return true; +} - rct_optimizations::CameraIntrinsics temp_intr; - if (!read(xml, "fx", temp_intr.fx())) return false; - if (!read(xml, "fy", temp_intr.fy())) return false; - if (!read(xml, "cx", temp_intr.cx())) return false; - if (!read(xml, "cy", temp_intr.cy())) return false; +rct_optimizations::CameraIntrinsics rct_ros_tools::loadIntrinsics(const std::string& path) +{ + try + { + YAML::Node n = YAML::LoadFile(path); + + rct_optimizations::CameraIntrinsics intrinsics; + intrinsics.cx() = n["cx"].as(); + intrinsics.cy() = n["cy"].as(); + intrinsics.fx() = n["fx"].as(); + intrinsics.fy() = n["fy"].as(); + return intrinsics; + } + catch (YAML::Exception &ex) + { + throw BadFileException(std::string("YAML failure: ") + ex.what()); + } +} - intr = temp_intr; +bool rct_ros_tools::loadIntrinsics(const std::string& path, rct_optimizations::CameraIntrinsics& intrinsics) +{ + try + { + intrinsics = loadIntrinsics(path); + } + catch (BadFileException &ex) + { + ROS_ERROR_STREAM("Failed to load intrinsics from file: " << ex.what()); + return false; + } return true; } -bool rct_ros_tools::loadPose(const ros::NodeHandle& nh, const std::string& key, - Eigen::Isometry3d& pose) +Eigen::Isometry3d rct_ros_tools::loadPose(const ros::NodeHandle &nh, const std::string &key) { XmlRpc::XmlRpcValue xml; - if (!nh.getParam(key, xml)) return false; + if (!nh.getParam(key, xml)) throw ros::InvalidParameterException(key); - pose = Eigen::Isometry3d::Identity(); double x, y, z, qx, qy, qz, qw; - if (!read(xml, "x", x)) return false; - if (!read(xml, "y", y)) return false; - if (!read(xml, "z", z)) return false; - if (!read(xml, "qx", qx)) return false; - if (!read(xml, "qy", qy)) return false; - if (!read(xml, "qz", qz)) return false; - if (!read(xml, "qw", qw)) return false; - - pose.translation() = Eigen::Vector3d(x, y, z); - pose.linear() = Eigen::Quaterniond(qw, qx, qy, qz).toRotationMatrix(); + if (!read(xml, "x", x)) throw ros::InvalidParameterException(key + "/x"); + if (!read(xml, "y", y)) throw ros::InvalidParameterException(key + "/y"); + if (!read(xml, "z", z)) throw ros::InvalidParameterException(key + "/z"); + if (!read(xml, "qx", qx)) throw ros::InvalidParameterException(key + "/qx"); + if (!read(xml, "qy", qy)) throw ros::InvalidParameterException(key + "/qy"); + if (!read(xml, "qz", qz)) throw ros::InvalidParameterException(key + "/qz"); + if (!read(xml, "qw", qw)) throw ros::InvalidParameterException(key + "/qw"); + + Eigen::Isometry3d pose = + Eigen::Translation3d(Eigen::Vector3d(x, y, z)) * + Eigen::AngleAxisd(Eigen::Quaterniond(qw, qx, qy, qz)); + return pose; +} +bool rct_ros_tools::loadPose(const ros::NodeHandle& nh, const std::string& key, Eigen::Isometry3d& pose) +{ + try + { + pose = loadPose(nh, key); + } + catch (ros::InvalidParameterException &ex) + { + ROS_ERROR_STREAM("Failed to load pose parameter: " << ex.what()); + return false; + } return true; } +Eigen::Isometry3d rct_ros_tools::loadPose(const std::string& path) +{ + try + { + YAML::Node n = YAML::LoadFile(path); + + Eigen::Vector3d position; + position(0) = n["x"].as(); + position(1) = n["y"].as(); + position(2) = n["z"].as(); + + Eigen::Quaterniond quat; + quat.w() = n["qw"].as(); + quat.x() = n["qx"].as(); + quat.y() = n["qy"].as(); + quat.z() = n["qz"].as(); + + Eigen::Isometry3d pose = Eigen::Translation3d(position) * Eigen::AngleAxisd(quat); + return pose; + } + catch (YAML::Exception &ex) + { + throw BadFileException(std::string("YAML failure: ") + ex.what()); + } +} + bool rct_ros_tools::loadPose(const std::string& path, Eigen::Isometry3d& pose) { - YAML::Node n = YAML::LoadFile(path); - Eigen::Vector3d position; - - position(0) = n["x"].as(); - position(1) = n["y"].as(); - position(2) = n["z"].as(); - - double qw, qx, qy, qz; - qw = n["qw"].as(); - qx = n["qx"].as(); - qy = n["qy"].as(); - qz = n["qz"].as(); - - pose = Eigen::Isometry3d::Identity(); - pose.translation() = position; - pose.linear() = Eigen::Quaterniond(qw, qx, qy, qz).toRotationMatrix(); + try + { + pose = loadPose(path); + } + catch (BadFileException &ex) + { + ROS_ERROR_STREAM("Failed to load pose from file: " << ex.what()); + return false; + } return true; }