From 0f6afb06d3fe379c9b756283739799e7f6d5f5c1 Mon Sep 17 00:00:00 2001 From: Abhay Deshpande Date: Mon, 8 Aug 2022 23:10:14 -0700 Subject: [PATCH] Created camera configs, tuned more arm (#166) --- camera-config/ForearmCameraCalibration.yml | 36 ++++++++++++++++++++ camera-config/HandCameraCalibration.yml | 36 ++++++++++++++++++++ camera-config/MastCameraCalibration.yml | 2 +- src/Constants.h | 8 +++++ src/network/MissionControlProtocol.cpp | 21 +++++++----- src/world_interface/real_world_constants.h | 8 ++--- src/world_interface/real_world_interface.cpp | 26 +++++++++++--- 7 files changed, 118 insertions(+), 19 deletions(-) create mode 100644 camera-config/ForearmCameraCalibration.yml create mode 100644 camera-config/HandCameraCalibration.yml diff --git a/camera-config/ForearmCameraCalibration.yml b/camera-config/ForearmCameraCalibration.yml new file mode 100644 index 000000000..b35bb5ffc --- /dev/null +++ b/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.] diff --git a/camera-config/HandCameraCalibration.yml b/camera-config/HandCameraCalibration.yml new file mode 100644 index 000000000..158cddb9b --- /dev/null +++ b/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.] diff --git a/camera-config/MastCameraCalibration.yml b/camera-config/MastCameraCalibration.yml index 6738ec07b..b812719dd 100644 --- a/camera-config/MastCameraCalibration.yml +++ b/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 diff --git a/src/Constants.h b/src/Constants.h index 669862b61..b45d91e97 100644 --- a/src/Constants.h +++ b/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 */ diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp index 839c0744d..310c76b60 100644 --- a/src/network/MissionControlProtocol.cpp +++ b/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 diff --git a/src/world_interface/real_world_constants.h b/src/world_interface/real_world_constants.h index a1dfc9576..fa9a8ffcc 100644 --- a/src/world_interface/real_world_constants.h +++ b/src/world_interface/real_world_constants.h @@ -83,7 +83,7 @@ constexpr auto motorPulsesPerJointRevMap = frozen::make_unordered_map({{motorid_t::armBase, 0.1831}, + frozen::make_unordered_map({{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::armBase, 0.1831}, + frozen::make_unordered_map({{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 diff --git a/src/world_interface/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp index 76bd6315a..294b2e7b8 100644 --- a/src/world_interface/real_world_interface.cpp +++ b/src/world_interface/real_world_interface.cpp @@ -82,15 +82,31 @@ void initMotors() { void setupCameras() { try { - auto arCam = std::make_shared(); - arCam->openFromConfigFile(Constants::AR_CAMERA_CONFIG_PATH); - cameraMap[Constants::AR_CAMERA_ID] = arCam; + auto cam = std::make_shared(); + 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->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->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