Created camera configs, tuned more arm (#166)

camera-config/ForearmCameraCalibration.yml

@@ -0,0 +1,36 @@
+%YAML:1.0
+---
+name: "forearm"
+description: "Camera located on the forearm of the rover."
+# Make sure that this camera_id matches the one defined in the udev rules.
+camera_id: 30
+calib_info:
+   calibration_time: "Sat 01 May 2021 01:54:16 PM PDT"
+   board_width: 11
+   board_height: 8
+   square_size: 20.
+   flags: 0
+   avg_reprojection_error: 4.5744250014767379e-01
+intrinsic_params:
+   image_width: 640
+   image_height: 480
+   camera_matrix: !!opencv-matrix
+      rows: 3
+      cols: 3
+      dt: d
+      data: [ 6.4936764773277400e+02, 0., 3.3546152305967058e+02, 0.,
+          6.4776804982533793e+02, 2.4785660889077488e+02, 0., 0., 1. ]
+   distortion_coefficients: !!opencv-matrix
+      rows: 5
+      cols: 1
+      dt: d
+      data: [ -4.5670452858419575e-01, 2.1512832208218641e-01,
+          -1.1179480533495888e-03, -5.7450430930239835e-04, 0. ]
+extrinsic_params: !!opencv-matrix
+  rows: 4
+  cols: 4
+  dt: d
+  data: [ 0., 0., 1., -.50,
+         -1., 0., 0., -.25,
+          0., 0., 0., 1.,
+          0., 0., 0., 0.]

camera-config/HandCameraCalibration.yml

@@ -0,0 +1,36 @@
+%YAML:1.0
+---
+name: "hand"
+description: "Camera located on the hand of the rover."
+# Make sure that this camera_id matches the one defined in the udev rules.
+camera_id: 40
+calib_info:
+   calibration_time: "Sat 01 May 2021 01:54:16 PM PDT"
+   board_width: 11
+   board_height: 8
+   square_size: 20.
+   flags: 0
+   avg_reprojection_error: 4.5744250014767379e-01
+intrinsic_params:
+   image_width: 640
+   image_height: 480
+   camera_matrix: !!opencv-matrix
+      rows: 3
+      cols: 3
+      dt: d
+      data: [ 6.4936764773277400e+02, 0., 3.3546152305967058e+02, 0.,
+          6.4776804982533793e+02, 2.4785660889077488e+02, 0., 0., 1. ]
+   distortion_coefficients: !!opencv-matrix
+      rows: 5
+      cols: 1
+      dt: d
+      data: [ -4.5670452858419575e-01, 2.1512832208218641e-01,
+          -1.1179480533495888e-03, -5.7450430930239835e-04, 0. ]
+extrinsic_params: !!opencv-matrix
+  rows: 4
+  cols: 4
+  dt: d
+  data: [ 0., 0., 1., -.50,
+         -1., 0., 0., -.25,
+          0., 0., 0., 1.,
+          0., 0., 0., 0.]

camera-config/MastCameraCalibration.yml

@@ -3,7 +3,7 @@
 name: "mast"
 description: "Camera located on the mast of the rover."
 # Make sure that this camera_id matches the one defined in the udev rules.
-camera_id: 0
+camera_id: 20
 calib_info:
    calibration_time: "Sat 01 May 2021 01:54:16 PM PDT"
    board_width: 11

src/Constants.h

@@ -63,9 +63,17 @@ constexpr double ELBOW_MIN = 0.0;
 constexpr double ELBOW_MAX = M_PI * 29. / 30.; // I think this should prevent self-collisions
 
 // TODO: We need to recalibrate the camera, since we replaced it with a different one.
+// TODO: rename cameras (in MC as well) as appropriate
 constexpr const char* AR_CAMERA_CONFIG_PATH = "../camera-config/MastCameraCalibration.yml";
 const robot::types::CameraID AR_CAMERA_ID = "front"; // TODO: replace with real camera name
 
+constexpr const char* FOREARM_CAMERA_CONFIG_PATH = "../camera-config/ForearmCameraCalibration.yml";
+const robot::types::CameraID FOREARM_CAMERA_ID = "rear";
+
+constexpr const char* HAND_CAMERA_CONFIG_PATH = "../camera-config/HandCameraCalibration.yml";
+const robot::types::CameraID HAND_CAMERA_ID = "upperArm";
+
+
 /**
    @deprecated No need for this constant once we fully switch over the Mission Control PlanViz
  */

src/network/MissionControlProtocol.cpp

@@ -352,15 +352,18 @@ void MissionControlProtocol::videoStreamTask() {
 			const uint32_t& frame_num = stream.second;
 			if (robot::hasNewCameraFrame(cam, frame_num)) {
 				// if there is a new frame, grab it
-				auto data = robot::readCamera(cam).getData();
-				uint32_t& new_frame_num = data.second;
-				cv::Mat frame = data.first;
-				// update the previous frame number
-				this->_open_streams[cam] = new_frame_num;
-
-				// convert frame to base64 and send it
-				std::string b64_data = base64::encodeMat(frame, ".jpg");
-				sendCameraStreamReport(cam, b64_data);
+				auto camDP = robot::readCamera(cam);
+				if (camDP) {
+					auto data = camDP.getData();
+					uint32_t& new_frame_num = data.second;
+					cv::Mat frame = data.first;
+					// update the previous frame number
+					this->_open_streams[cam] = new_frame_num;
+
+					// convert frame to base64 and send it
+					std::string b64_data = base64::encodeMat(frame, ".jpg");
+					sendCameraStreamReport(cam, b64_data);
+				}
 			}
 
 			// break out of the loop if we should stop streaming

src/world_interface/real_world_constants.h

@@ -83,7 +83,7 @@ constexpr auto motorPulsesPerJointRevMap = frozen::make_unordered_map<motorid_t,
  * Negative values mean that the motor is inverted.
 */
 constexpr auto positive_pwm_scales =
-	frozen::make_unordered_map<motorid_t, double>({{motorid_t::armBase, 0.1831},
+	frozen::make_unordered_map<motorid_t, double>({{motorid_t::armBase, 0.5},
 												   {motorid_t::shoulder, -1},
 												   {motorid_t::elbow, 0.75},
 												   {motorid_t::forearm, 0.1},
@@ -93,13 +93,13 @@ constexpr auto positive_pwm_scales =
 												   {motorid_t::frontRightWheel, 0.5},
 												   {motorid_t::rearLeftWheel, -0.5},
 												   {motorid_t::rearRightWheel, -0.5},
-												   {motorid_t::hand, 0.2289}});
+												   {motorid_t::hand, 0.75}});
 /**
  * @brief A mapping of motorids to power scale factors when commanded with negative power.
  * Negative values mean that the motor is inverted.
 */
 constexpr auto negative_pwm_scales =
-	frozen::make_unordered_map<motorid_t, double>({{motorid_t::armBase, 0.1831},
+	frozen::make_unordered_map<motorid_t, double>({{motorid_t::armBase, 0.5},
 												   {motorid_t::shoulder, -1},
 												   {motorid_t::elbow, 0.75},
 												   {motorid_t::forearm, 0.1},
@@ -109,6 +109,6 @@ constexpr auto negative_pwm_scales =
 												   {motorid_t::frontRightWheel, 0.5},
 												   {motorid_t::rearLeftWheel, -0.5},
 												   {motorid_t::rearRightWheel, -0.5},
-												   {motorid_t::hand, 0.2289}});
+												   {motorid_t::hand, 0.75}});
 
 } // namespace robot

src/world_interface/real_world_interface.cpp

@@ -82,15 +82,31 @@ void initMotors() {
 
 void setupCameras() {
 	try {
-		auto arCam = std::make_shared<cam::Camera>();
-		arCam->openFromConfigFile(Constants::AR_CAMERA_CONFIG_PATH);
-		cameraMap[Constants::AR_CAMERA_ID] = arCam;
+		auto cam = std::make_shared<cam::Camera>();
+		cam->openFromConfigFile(Constants::AR_CAMERA_CONFIG_PATH);
+		cameraMap[Constants::AR_CAMERA_ID] = cam;
+	} catch (const std::exception& e) {
+		log(LOG_ERROR, "Error opening camera with id %s:\n%s\n",
+			util::to_string(Constants::AR_CAMERA_ID).c_str(), e.what());
+	}
+	/* TODO: see why only 2 cams can be open at a time
+	try {
+		auto cam = std::make_shared<cam::Camera>();
+		cam->openFromConfigFile(Constants::FOREARM_CAMERA_CONFIG_PATH);
+		cameraMap[Constants::FOREARM_CAMERA_ID] = cam;
+	} catch (const std::exception& e) {
+		log(LOG_ERROR, "Error opening camera with id %s:\n%s\n",
+			util::to_string(Constants::AR_CAMERA_ID).c_str(), e.what());
+	}
+	*/
+	try {
+		auto cam = std::make_shared<cam::Camera>();
+		cam->openFromConfigFile(Constants::HAND_CAMERA_CONFIG_PATH);
+		cameraMap[Constants::HAND_CAMERA_ID] = cam;
 	} catch (const std::exception& e) {
 		log(LOG_ERROR, "Error opening camera with id %s:\n%s\n",
 			util::to_string(Constants::AR_CAMERA_ID).c_str(), e.what());
 	}
-
-	// Set up the rest of the cameras here
 }
 } // namespace