samples: update mecheye_ros_interface to SDK 2.0.1

MechMindRobotics · Dec 16, 2022 · f71b053 · f71b053
1 parent ea97a05
commit f71b053
Show file tree

Hide file tree

Showing 11 changed files with 223 additions and 15 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -49,6 +49,11 @@ add_service_files(
   GetUhpCaptureMode.srv
   GetUhpFringeCodingMode.srv
   GetUhpSettings.srv
+
+  GetProjectorFringeCodingMode.srv
+  GetProjectorPowerLevel.srv
+  GetProjectorAntiFlickerMode.srv
+
   SaveAllSettingsToUserSets.srv
   Set2DExpectedGrayValue.srv
   Set2DExposureMode.srv
@@ -70,6 +75,10 @@ add_service_files(
   SetUhpCaptureMode.srv
   SetUhpFringeCodingMode.srv
   SetUhpSettings.srv
+
+  SetProjectorFringeCodingMode.srv
+  SetProjectorPowerLevel.srv
+  SetProjectorAntiFlickerMode.srv
 )
 
 generate_messages(

diff --git a/include/MechMindCamera.h b/include/MechMindCamera.h
@@ -28,6 +28,10 @@
 #include <mecheye_ros_interface/GetUhpCaptureMode.h>
 #include <mecheye_ros_interface/GetUhpFringeCodingMode.h>
 #include <mecheye_ros_interface/GetUhpSettings.h>
+#include <mecheye_ros_interface/GetProjectorFringeCodingMode.h>
+#include <mecheye_ros_interface/GetProjectorPowerLevel.h>
+#include <mecheye_ros_interface/GetProjectorAntiFlickerMode.h>
+
 #include <mecheye_ros_interface/SaveAllSettingsToUserSets.h>
 #include <mecheye_ros_interface/Set2DExpectedGrayValue.h>
 #include <mecheye_ros_interface/Set2DExposureMode.h>
@@ -50,6 +54,9 @@
 #include <mecheye_ros_interface/SetUhpFringeCodingMode.h>
 #include <mecheye_ros_interface/SetUhpSettings.h>
 
+#include <mecheye_ros_interface/SetProjectorFringeCodingMode.h>
+#include <mecheye_ros_interface/SetProjectorPowerLevel.h>
+#include <mecheye_ros_interface/SetProjectorAntiFlickerMode.h>
 
 namespace mecheye_ros_interface
 {
@@ -120,7 +127,12 @@ namespace mecheye_ros_interface
         ros::ServiceServer get_uhp_capture_mode_service;
         ros::ServiceServer get_uhp_fringe_coding_mode_service;
         ros::ServiceServer get_uhp_settings_service;
-        ros::ServiceServer save_all_settings_to_user_sets_service;
+
+        ros::ServiceServer get_projector_fringecodingmode_service;
+        ros::ServiceServer get_projector_powerlevel_service;
+        ros::ServiceServer get_projector_antiflickermode_service;
+
+	ros::ServiceServer save_all_settings_to_user_sets_service;
         ros::ServiceServer set_2d_expected_gray_value_service;
         ros::ServiceServer set_2d_exposure_mode_service;
         ros::ServiceServer set_2d_exposure_sequence_service;
@@ -142,6 +154,10 @@ namespace mecheye_ros_interface
         ros::ServiceServer set_uhp_fringe_coding_mode_service;
         ros::ServiceServer set_uhp_settings_service;
 
+        ros::ServiceServer set_projector_fringecodingmode_service;
+        ros::ServiceServer set_projector_powerlevel_service;
+        ros::ServiceServer set_projector_antiflickermode_service;
+
         bool add_user_set_callback(AddUserSet::Request &req, AddUserSet::Response &res);
         bool capture_color_map_callback(CaptureColorMap::Request &req, CaptureColorMap::Response &res);
         bool capture_color_point_cloud_callback(CaptureColorPointCloud::Request &req, CaptureColorPointCloud::Response &res);
@@ -173,7 +189,12 @@ namespace mecheye_ros_interface
         bool get_uhp_settings_callback(GetUhpSettings::Request &req, GetUhpSettings::Response &res);
         bool get_uhp_capture_mode_callback(GetUhpCaptureMode::Request &req, GetUhpCaptureMode::Response &res);
         bool get_uhp_fringe_coding_mode_callback(GetUhpFringeCodingMode::Request &req, GetUhpFringeCodingMode::Response &res);
-        bool save_all_settings_to_user_sets_callback(SaveAllSettingsToUserSets::Request &req,
+
+        bool get_projector_fringecodingmode_callback(GetProjectorFringeCodingMode::Request &req, GetProjectorFringeCodingMode::Response &res);
+        bool get_projector_powerlevel_callback(GetProjectorPowerLevel::Request &req, GetProjectorPowerLevel::Response &res);
+        bool get_projector_antiflickermode_callback(GetProjectorAntiFlickerMode::Request &req, GetProjectorAntiFlickerMode::Response &res);
+
+	bool save_all_settings_to_user_sets_callback(SaveAllSettingsToUserSets::Request &req,
                                                      SaveAllSettingsToUserSets::Response &res);
         bool set_2d_expected_gray_value_callback(Set2DExpectedGrayValue::Request &req,
                                                  Set2DExpectedGrayValue::Response &res);
@@ -198,5 +219,9 @@ namespace mecheye_ros_interface
         bool set_uhp_settings_callback(SetUhpSettings::Request &req, SetUhpSettings::Response &res);
         bool set_uhp_capture_mode_callback(SetUhpCaptureMode::Request &req, SetUhpCaptureMode::Response &res);
         bool set_uhp_fringe_coding_mode_callback(SetUhpFringeCodingMode::Request &req, SetUhpFringeCodingMode::Response &res);
+
+	bool set_projector_fringecodingmode_callback(SetProjectorFringeCodingMode::Request &req, SetProjectorFringeCodingMode::Response &res);
+        bool set_projector_powerlevel_callback(SetProjectorPowerLevel::Request &req, SetProjectorPowerLevel::Response &res);
+        bool set_projector_antiflickermode_callback(SetProjectorAntiFlickerMode::Request &req, SetProjectorAntiFlickerMode::Response &res);
     };
-} // namespace mecheye_ros_interface
+} // namespace mecheye_ros_interface
diff --git a/launch/start_camera.launch b/launch/start_camera.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="save_file" default="false" />
+    <arg name="save_file" default="true" />
     <arg name="camera_ip" default="172.24.144.1" />      <!-- change to your camera ip -->
     <arg name="use_external_intri" default="false" />
     <arg name="fx" default="1727.4641025602748" />

diff --git a/package.xml b/package.xml
@@ -4,10 +4,8 @@
   <version>0.0.2</version>
   <description>mecheye_ros_interface package for Mechmind 3D camera, modify from the official API from Mechmind</description>
 
-  <maintainer email="[email protected]">Hongzhuo Liang</maintainer>
+  <maintainer email="[email protected]"></maintainer>
   <license>MIT</license>
-  <url type="website">tams.informatik.uni-hamburg.de</url>
-  <author email="[email protected]">Hongzhuo Liang</author>
 
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscpp</build_depend>

diff --git a/src/MechMindCamera.cpp b/src/MechMindCamera.cpp
@@ -159,6 +159,14 @@ namespace mecheye_ros_interface
             nh.advertiseService("get_uhp_settings", &MechMindCamera::get_uhp_settings_callback, this);
         save_all_settings_to_user_sets_service = nh.advertiseService(
             "save_all_settings_to_user_sets", &MechMindCamera::save_all_settings_to_user_sets_callback, this);
+
+        get_projector_fringecodingmode_service =
+            nh.advertiseService("get_projector_fringecodingmode", &MechMindCamera::get_projector_fringecodingmode_callback, this);
+        get_projector_powerlevel_service =
+            nh.advertiseService("get_projector_powerlevel", &MechMindCamera::get_projector_powerlevel_callback, this);
+        get_projector_antiflickermode_service =
+            nh.advertiseService("get_projector_antiflickermode", &MechMindCamera::get_projector_antiflickermode_callback, this);
+
         set_2d_expected_gray_value_service =
             nh.advertiseService("set_2d_expected_gray_value", &MechMindCamera::set_2d_expected_gray_value_callback, this);
         set_2d_exposure_mode_service =
@@ -194,6 +202,14 @@ namespace mecheye_ros_interface
             nh.advertiseService("set_uhp_fringe_coding_mode", &MechMindCamera::set_uhp_fringe_coding_mode_callback, this);
         set_uhp_settings_service =
             nh.advertiseService("set_uhp_settings", &MechMindCamera::set_uhp_settings_callback, this);
+
+
+        set_projector_fringecodingmode_service =
+            nh.advertiseService("set_projector_fringecodingmode", &MechMindCamera::set_projector_fringecodingmode_callback, this);
+        set_projector_powerlevel_service =
+            nh.advertiseService("set_projector_powerlevel", &MechMindCamera::set_projector_powerlevel_callback, this);
+        set_projector_antiflickermode_service =
+            nh.advertiseService("set_projector_antiflickermode", &MechMindCamera::set_projector_antiflickermode_callback, this);
     }
 
     void MechMindCamera::publishColorMap(mmind::api::ColorMap &colorMap)
@@ -617,7 +633,7 @@ namespace mecheye_ros_interface
         mmind::api::LaserSettings laserSettings;
         mmind::api::ErrorStatus status = device.getLaserSettings(laserSettings);
         showError(status);
-        switch (laserSettings.FringeCodingMode)
+        switch (laserSettings.fringeCodingMode)
         {
         case 0:
             res.fringeCodingMode = "Fast";
@@ -631,10 +647,10 @@ namespace mecheye_ros_interface
             res.fringeCodingMode = "";
             break;
         }
-        res.frameRangeStart = laserSettings.FrameRangeStart;
-        res.frameRangeEnd = laserSettings.FrameRangeEnd;
-        res.framePartitionCount = laserSettings.FramePartitionCount;
-        res.powerLevel = laserSettings.PowerLevel;
+        res.frameRangeStart = laserSettings.frameRangeStart;
+        res.frameRangeEnd = laserSettings.frameRangeEnd;
+        res.framePartitionCount = laserSettings.framePartitionCount;
+        res.powerLevel = laserSettings.powerLevel;
         return status.isOK();
     }
 
@@ -643,7 +659,7 @@ namespace mecheye_ros_interface
         mmind::api::UhpSettings uhpSettings;
         mmind::api::ErrorStatus status = device.getUhpSettings(uhpSettings);
         showError(status);
-        switch (uhpSettings.CaptureMode)
+        switch (uhpSettings.captureMode)
         {
         case mmind::api::UhpSettings::UhpCaptureMode::Camera1:
             res.capture_mode = "Camera1";
@@ -661,7 +677,7 @@ namespace mecheye_ros_interface
             res.capture_mode = "";
             break;
         }
-        switch (uhpSettings.FringeCodingMode)
+        switch (uhpSettings.fringeCodingMode)
         {
         case mmind::api::UhpSettings::UhpFringeCodingMode::Fast:
             res.fringe_coding_mode = "Fast";
@@ -726,6 +742,81 @@ namespace mecheye_ros_interface
         return status.isOK();
     }
 
+        bool MechMindCamera::get_projector_fringecodingmode_callback(GetProjectorFringeCodingMode::Request &req, GetProjectorFringeCodingMode::Response &res)
+    {
+        mmind::api::ProjectorSettings::FringeCodingMode mode;
+        mmind::api::ErrorStatus status = device.getProjectorFringeCodingMode(mode);
+        showError(status);
+        switch (mode)
+        {
+        case mmind::api::ProjectorSettings::FringeCodingMode::Fast:
+            res.value = "Fast";
+            break;
+
+        case mmind::api::ProjectorSettings::FringeCodingMode::Accurate:
+            res.value = "Accurate";
+            break;
+
+        default:
+            res.value = "";
+            break;
+        }
+        return status.isOK();
+    }
+
+    bool MechMindCamera::get_projector_powerlevel_callback(GetProjectorPowerLevel::Request &req, GetProjectorPowerLevel::Response &res)
+    {
+        mmind::api::ProjectorSettings::PowerLevel mode;
+        mmind::api::ErrorStatus status = device.getProjectorPowerLevel(mode);
+        showError(status);
+        switch (mode)
+        {
+        case mmind::api::ProjectorSettings::PowerLevel::High:
+            res.value = "High";
+            break;
+
+        case mmind::api::ProjectorSettings::PowerLevel::Normal:
+            res.value = "Normal";
+            break;
+
+        case mmind::api::ProjectorSettings::PowerLevel::Low:
+            res.value = "Low";
+            break;
+
+        default:
+            res.value = "";
+            break;
+        }
+        return status.isOK();
+    }
+
+    bool MechMindCamera::get_projector_antiflickermode_callback(GetProjectorAntiFlickerMode::Request &req, GetProjectorAntiFlickerMode::Response &res)
+    {
+        mmind::api::ProjectorSettings::AntiFlickerMode mode;
+        mmind::api::ErrorStatus status = device.getProjectorAntiFlickerMode(mode);
+        showError(status);
+        switch (mode)
+        {
+        case mmind::api::ProjectorSettings::AntiFlickerMode::Off:
+            res.value = "Off";
+            break;
+
+        case mmind::api::ProjectorSettings::AntiFlickerMode::AC50Hz:
+            res.value = "AC50Hz";
+            break;
+
+        case mmind::api::ProjectorSettings::AntiFlickerMode::AC60Hz:
+            res.value = "AC60Hz";
+            break;
+
+        default:
+            res.value = "";
+            break;
+        }
+        return status.isOK();
+    }
+
+
     bool MechMindCamera::save_all_settings_to_user_sets_callback(SaveAllSettingsToUserSets::Request &req,
                                                                  SaveAllSettingsToUserSets::Response &res)
     {
@@ -977,7 +1068,7 @@ namespace mecheye_ros_interface
     }
 
     bool MechMindCamera::set_uhp_fringe_coding_mode_callback(SetUhpFringeCodingMode::Request &req, SetUhpFringeCodingMode::Response &res)
-    {
+    {  
         mmind::api::UhpSettings::UhpFringeCodingMode mode;
         if (req.fringe_coding_mode == "Fast")
             mode = mmind::api::UhpSettings::UhpFringeCodingMode::Fast;
@@ -995,4 +1086,68 @@ namespace mecheye_ros_interface
         res.error_description = status.errorDescription.c_str();
         return true;
     }
+
+    bool MechMindCamera::set_projector_fringecodingmode_callback(SetProjectorFringeCodingMode::Request &req, SetProjectorFringeCodingMode::Response &res)
+    {
+        mmind::api::ProjectorSettings::FringeCodingMode mode;
+        if (req.value == "Fast")
+            mode = mmind::api::ProjectorSettings::FringeCodingMode::Fast;
+        else if (req.value == "Accurate")
+            mode = mmind::api::ProjectorSettings::FringeCodingMode::Accurate;
+        else
+        {
+            res.errorCode = mmind::api::ErrorStatus::ErrorCode::MMIND_STATUS_PARAMETER_SET_ERROR;
+            res.errorDescription = "Invalid parameter. Valid choices include '!!str Fast', 'Accurate'.";
+            return true;
+        }
+        mmind::api::ErrorStatus status = device.setProjectorFringeCodingMode(mode);
+        showError(status);
+        res.errorCode = status.errorCode;
+        res.errorDescription = status.errorDescription.c_str();
+        return true;
+    }
+
+    bool MechMindCamera::set_projector_powerlevel_callback(SetProjectorPowerLevel::Request &req, SetProjectorPowerLevel::Response &res)
+    {
+        mmind::api::ProjectorSettings::PowerLevel mode;
+        if (req.value == "High")
+            mode = mmind::api::ProjectorSettings::PowerLevel::High;
+        else if (req.value == "Normal")
+            mode = mmind::api::ProjectorSettings::PowerLevel::Normal;
+        else if (req.value == "Low")
+            mode = mmind::api::ProjectorSettings::PowerLevel::Low;
+        else
+        {
+            res.errorCode = mmind::api::ErrorStatus::ErrorCode::MMIND_STATUS_PARAMETER_SET_ERROR;
+            res.errorDescription = "Invalid parameter. Valid choices include '!!str High', 'Normal', 'Low'.";
+            return true;
+        }
+        mmind::api::ErrorStatus status = device.setProjectorPowerLevel(mode);
+        showError(status);
+        res.errorCode = status.errorCode;
+        res.errorDescription = status.errorDescription.c_str();
+        return true;
+    }
+
+    bool MechMindCamera::set_projector_antiflickermode_callback(SetProjectorAntiFlickerMode::Request &req, SetProjectorAntiFlickerMode::Response &res)
+    {
+        mmind::api::ProjectorSettings::AntiFlickerMode mode;
+        if (req.value == "Off")
+            mode = mmind::api::ProjectorSettings::AntiFlickerMode::Off;
+        else if (req.value == "AC50Hz")
+            mode = mmind::api::ProjectorSettings::AntiFlickerMode::AC50Hz;
+        else if (req.value == "AC60Hz")
+            mode = mmind::api::ProjectorSettings::AntiFlickerMode::AC60Hz;
+        else
+        {
+            res.errorCode = mmind::api::ErrorStatus::ErrorCode::MMIND_STATUS_PARAMETER_SET_ERROR;
+            res.errorDescription = "Invalid parameter. Valid choices include '!!str Off', 'AC50Hz', 'AC60Hz'.";
+            return true;
+        }
+        mmind::api::ErrorStatus status = device.setProjectorAntiFlickerMode(mode);
+        showError(status);
+        res.errorCode = status.errorCode;
+        res.errorDescription = status.errorDescription.c_str();
+        return true;
+    }
 } // namespace mecheye_ros_interface
diff --git a/srv/GetProjectorAntiFlickerMode.srv b/srv/GetProjectorAntiFlickerMode.srv
@@ -0,0 +1,2 @@
+---
+string value
diff --git a/srv/GetProjectorFringeCodingMode.srv b/srv/GetProjectorFringeCodingMode.srv
@@ -0,0 +1,2 @@
+---
+string value
diff --git a/srv/GetProjectorPowerLevel.srv b/srv/GetProjectorPowerLevel.srv
@@ -0,0 +1,2 @@
+---
+string value
diff --git a/srv/SetProjectorAntiFlickerMode.srv b/srv/SetProjectorAntiFlickerMode.srv
@@ -0,0 +1,5 @@
+# Off AC50Hz AC60Hz
+string value
+---
+int32 errorCode
+string errorDescription
diff --git a/srv/SetProjectorFringeCodingMode.srv b/srv/SetProjectorFringeCodingMode.srv
@@ -0,0 +1,5 @@
+# Fast Accurate
+string value
+---
+int32 errorCode
+string errorDescription
diff --git a/srv/SetProjectorPowerLevel.srv b/srv/SetProjectorPowerLevel.srv
@@ -0,0 +1,5 @@
+# High Normal Low
+string value
+---
+int32 errorCode
+string errorDescription