diff --git a/analyst/tools/query_view_points.cc b/analyst/tools/query_view_points.cc index 9efd2cf7..12fc2d0b 100644 --- a/analyst/tools/query_view_points.cc +++ b/analyst/tools/query_view_points.cc @@ -114,7 +114,7 @@ int main(int argc, char** argv) { new geometry_msgs::Transform(msg_conversions::eigen_transform_to_ros_transform(transform_body_to_cam))); camera::CameraParameters cam_params(&config, FLAGS_camera.c_str()); - inspection::CameraView camera(cam_params, 2.0, 0.19, msg_pointer); + inspection::CameraView camera(FLAGS_camera, cam_params, 2.0, 0.19, msg_pointer); // Extract the input list from the command-line argument diff --git a/astrobee/behaviors/inspection/include/inspection/camera_projection.h b/astrobee/behaviors/inspection/include/inspection/camera_projection.h index 07071b82..d9c41b85 100644 --- a/astrobee/behaviors/inspection/include/inspection/camera_projection.h +++ b/astrobee/behaviors/inspection/include/inspection/camera_projection.h @@ -69,9 +69,8 @@ class CameraView : public camera::CameraModel { // f: far clip, maximum camera distance (m) // n: near clip, minimum camera distance (m) // cam_transform: transform from body->camera, useful for offline applications where tf is not available - CameraView(const camera::CameraParameters & params, const float f = 2.0, const float n = 0.19, - const geometry_msgs::Transform::ConstPtr cam_transform = NULL); - + CameraView(const std::string& cam_name, const camera::CameraParameters& params, const float f = 2.0, + const float n = 0.19, const geometry_msgs::Transform::ConstPtr cam_transform = NULL); // Gets the points x y where the point is in the image. If outside the image, then it will return false // If the robot pose is not specified, it's considered to be the current one diff --git a/astrobee/behaviors/inspection/src/camera_projection.cc b/astrobee/behaviors/inspection/src/camera_projection.cc index 17071245..b9925eef 100644 --- a/astrobee/behaviors/inspection/src/camera_projection.cc +++ b/astrobee/behaviors/inspection/src/camera_projection.cc @@ -34,10 +34,11 @@ namespace inspection { the camera frame and the other way around. It automatically reads the camera parameters from the config files based on the camera name, such that no setup is necessary. */ -CameraView::CameraView(const camera::CameraParameters & params, const float f, const float n, - const geometry_msgs::Transform::ConstPtr cam_transform) - : camera::CameraModel(params), f_(f), n_(n) { - +CameraView::CameraView(const std::string& cam_name, const camera::CameraParameters& params, const float f, + const float n, const geometry_msgs::Transform::ConstPtr cam_transform) + : cam_name_(cam_name), camera::CameraModel(params), f_(f), n_(n) { + cfg_cam_.AddFile("cameras.config"); + if (!cfg_cam_.ReadFiles()) ROS_FATAL("Failed to read config files."); // Get relative camera position if (cam_transform == NULL) { // Create a transform buffer to listen for transforms @@ -147,7 +148,7 @@ bool CameraView::InsideTarget(std::vector vert_x, std::vector vert_y, double size_y) { // Create depth cam camera model static camera::CameraParameters depth_cam_params(&cfg_cam_, (depth_cam_name + "_cam").c_str()); - CameraView depth_cam(depth_cam_params, f_, n_); + CameraView depth_cam(depth_cam_name + "_cam", depth_cam_params, f_, n_); // Get most recent depth message std::string cam_prefix = TOPIC_HARDWARE_PICOFLEXX_PREFIX; diff --git a/astrobee/behaviors/inspection/src/inspection.cc b/astrobee/behaviors/inspection/src/inspection.cc index 36f1eecc..2646f13d 100644 --- a/astrobee/behaviors/inspection/src/inspection.cc +++ b/astrobee/behaviors/inspection/src/inspection.cc @@ -436,8 +436,9 @@ bool Inspection::GenerateAnomalySurvey(geometry_msgs::PoseArray &points_anomaly) curr_camera_ = cam_name.c_str(); if (cameras_.find(cam_name) == cameras_.end()) { static camera::CameraParameters cam_params(&cfg_cam_, cam_name.c_str()); - cameras_.emplace(std::piecewise_construct, std::make_tuple(cam_name), - std::make_tuple(cam_params, cfg_->Get("max_distance"), cfg_->Get("min_distance"))); + cameras_.emplace( + std::piecewise_construct, std::make_tuple(cam_name), + std::make_tuple(cam_name, cam_params, cfg_->Get("max_distance"), cfg_->Get("min_distance"))); } @@ -523,8 +524,9 @@ bool Inspection::GeneratePanoramaSurvey(geometry_msgs::PoseArray &points_panoram curr_camera_ = cam_name.c_str(); if (cameras_.find(cam_name) == cameras_.end()) { static camera::CameraParameters cam_params(&cfg_cam_, cam_name.c_str()); - cameras_.emplace(std::piecewise_construct, std::make_tuple(cam_name), - std::make_tuple(cam_params, cfg_->Get("max_distance"), cfg_->Get("min_distance"))); + cameras_.emplace( + std::piecewise_construct, std::make_tuple(cam_name), + std::make_tuple(cam_name, cam_params, cfg_->Get("max_distance"), cfg_->Get("min_distance"))); } geometry_msgs::PoseArray panorama_relative; diff --git a/astrobee/behaviors/inspection/tools/inspection_tool.cc b/astrobee/behaviors/inspection/tools/inspection_tool.cc index a131ef1a..e151bd6e 100644 --- a/astrobee/behaviors/inspection/tools/inspection_tool.cc +++ b/astrobee/behaviors/inspection/tools/inspection_tool.cc @@ -100,7 +100,7 @@ DEFINE_string(att, "", "Desired attitude in RPY format 'roll pitch yaw' (degrees // Plan files DEFINE_string(anomaly_poses, "/resources/inspection_iss.txt", "Vent pose list to inspect"); -DEFINE_string(geometry_poses, "/resources/survey_bay_6.txt", "Geometry poses list to map"); +DEFINE_string(geometry_poses, "/resources/geometry_iss.txt", "Geometry poses list to map"); DEFINE_string(panorama_poses, "/resources/panorama_iss.txt", "Panorama poses list to map"); DEFINE_string(volumetric_poses, "/resources/volumetric_iss.txt", "Wifi poses list to map"); @@ -390,7 +390,7 @@ void ConnectedCallback( if (!client->IsConnected()) return; // Print out a status message std::cout << "\r " - << "\rState: CONNECTED" << std::flush; + << "\rState: CONNECTED\n" << std::flush; SendGoal(client); } diff --git a/pano/pano_view/tools/find_point_coordinate.cc b/pano/pano_view/tools/find_point_coordinate.cc index 62d60058..801e1d1b 100644 --- a/pano/pano_view/tools/find_point_coordinate.cc +++ b/pano/pano_view/tools/find_point_coordinate.cc @@ -215,7 +215,7 @@ int main(int argc, char** argv) { new geometry_msgs::Transform(msg_conversions::eigen_transform_to_ros_transform(transform_body_to_cam))); camera::CameraParameters cam_params(&config, camera_name.c_str()); - inspection::CameraView camera(cam_params, 2.0, 0.19, msg_pointer); + inspection::CameraView camera(camera_name, cam_params, 2.0, 0.19, msg_pointer); camera.SetTransform((msg_conversions::ros_pose_to_eigen_transform(ground_truth) * transform_body_to_cam).inverse());