Fix cam name var (#105)

* making spawn changes and adding double handed cargo

* fixing bug where cam_name wasn't initialized

* adding things from review and some bugfixes

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

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

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<int> vert_x, std::vector<int> 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;

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<double>("max_distance"), cfg_->Get<double>("min_distance")));
+    cameras_.emplace(
+      std::piecewise_construct, std::make_tuple(cam_name),
+      std::make_tuple(cam_name, cam_params, cfg_->Get<double>("max_distance"), cfg_->Get<double>("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<double>("max_distance"), cfg_->Get<double>("min_distance")));
+    cameras_.emplace(
+      std::piecewise_construct, std::make_tuple(cam_name),
+      std::make_tuple(cam_name, cam_params, cfg_->Get<double>("max_distance"), cfg_->Get<double>("min_distance")));
   }
 
   geometry_msgs::PoseArray panorama_relative;

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);
 }
 

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());