Fixed workflow file and naming convention for functions

.github/workflows/main.yml

@@ -15,4 +15,21 @@ jobs:
       - run: sudo apt-get install -y libpciaccess-dev libomp-dev
       - uses: ros-tooling/[email protected]
         with:
+          package-name: test_calibration
           target-ros2-distro: humble
+          skip-tests: true
+      - uses: ros-tooling/[email protected]
+        with:
+          package-name: test_admittance
+          target-ros2-distro: humble
+          skip-tests: true
+      - uses: ros-tooling/[email protected]
+        with:
+          package-name: test_impedance
+          target-ros2-distro: humble
+          skip-tests: true
+      - uses: ros-tooling/[email protected]
+        with:
+          package-name: haptic_control
+          target-ros2-distro: humble
+          skip-tests: true

src/haptic_control/include/haptic_control.hpp

@@ -3,8 +3,8 @@
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/LinearMath/Quaternion.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2_ros/transform_broadcaster.h"
 
 #include <Eigen/Core>
@@ -34,7 +34,7 @@ using std::placeholders::_2;
 using namespace std::chrono_literals;
 
 class HapticControl : public rclcpp::Node {
-public:
+ public:
   HapticControl(const std::string &name = "haptic_control",
                 const std::string &namespace_ = "",
                 const rclcpp::NodeOptions &options =
@@ -53,12 +53,12 @@ class HapticControl : public rclcpp::Node {
       const raptor_api_interfaces::msg::OutVirtuosePose::SharedPtr msg);
   void out_virtuose_statusCB(
       const raptor_api_interfaces::msg::OutVirtuoseStatus::SharedPtr msg);
-  void call_impedance_service();
+  void callImpedanceService();
   void impedanceThread();
-  void project_target_on_sphere(Eigen::Vector3d &target_position_vec,
-                                double safety_sphere_radius_);
+  void projectTargetOnSphere(Eigen::Vector3d &target_position_vec,
+                             double safety_sphere_radius_);
 
-private:
+ private:
   // ROS2 subscribtions
   rclcpp::Subscription<raptor_api_interfaces::msg::OutVirtuoseStatus>::SharedPtr
       out_virtuose_status_;
@@ -131,4 +131,4 @@ class HapticControl : public rclcpp::Node {
   Eigen::Vector3d x_tilde_old_, x_tilde_new_;
 };
 
-#endif // HAPTIC_CONTROL_HPP
+#endif  // HAPTIC_CONTROL_HPP

src/haptic_control/src/haptic_control.cpp

@@ -254,7 +254,7 @@ void HapticControl::out_virtuose_statusCB(
   status_button_ = msg->buttons;
 }
 
-void HapticControl::call_impedance_service() {
+void HapticControl::callImpedanceService() {
   received_ee_pose_ = false;
   while (!received_ee_pose_) {
     try {
@@ -444,7 +444,7 @@ void HapticControl::impedanceThread() {
                          target_position_vec.norm(), safety_sphere_radius_);
     if (target_position_vec.norm() > safety_sphere_radius_) {
       // project the target on the safety sphere to prevent singularities
-      project_target_on_sphere(target_position_vec, safety_sphere_radius_);
+      projectTargetOnSphere(target_position_vec, safety_sphere_radius_);
     } else {
       // update the old pose only if the target is within the safety sphere
       x_tilde_old_ = x_tilde_new_;
@@ -485,8 +485,8 @@ void HapticControl::impedanceThread() {
   tf_broadcaster_->sendTransform(target_pose_tf_);
   target_pos_publisher_->publish(target_pose_);
 }
-void HapticControl::project_target_on_sphere(
-    Eigen::Vector3d &target_position_vec, double safety_sphere_radius_) {
+void HapticControl::projectTargetOnSphere(Eigen::Vector3d &target_position_vec,
+                                          double safety_sphere_radius_) {
   target_position_vec =
       target_position_vec.normalized() * safety_sphere_radius_;
 }
@@ -498,7 +498,7 @@ int main(int argc, char **argv) {
 
   RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling impedance service:");
 
-  node->call_impedance_service();
+  node->callImpedanceService();
   rclcpp::spin(node);
   rclcpp::shutdown();
   return 0;

src/test_impedance/src/test_impedance.cpp

@@ -52,7 +52,7 @@ class TestImpedance : public rclcpp::Node {
         "virtuose_impedance");
 
     RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Calling impedance service:");
-    call_impedance_service();
+    callImpedanceService();
   }
 
  private:
@@ -86,7 +86,7 @@ class TestImpedance : public rclcpp::Node {
     status_button = msg->buttons;
   }
 
-  void call_impedance_service() {
+  void callImpedanceService() {
     // Request impedance mode
     RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sending impedance request");
 

src/vf_control/include/haptic_control_base.hpp

@@ -30,7 +30,7 @@
 #include "tf2_ros/transform_broadcaster.h"
 
 #include "utils.hpp"
-#include "vf_enforcer.hpp"
+#include "vf/vf_enforcer.hpp"
 
 class HapticControlBase : public rclcpp::Node {
  public:
@@ -48,15 +48,15 @@ class HapticControlBase : public rclcpp::Node {
   void set_safety_box_length_CB(const rclcpp::Parameter &p);
   void set_safety_box_height_CB(const rclcpp::Parameter &p);
   void update_current_ee_pos();
-  void SetWrenchCB(const geometry_msgs::msg::WrenchStamped target_wrench);
+  void set_wrench(const geometry_msgs::msg::WrenchStamped target_wrench);
   void out_virtuose_pose_CB(
       const raptor_api_interfaces::msg::OutVirtuosePose::SharedPtr msg);
   void out_virtuose_statusCB(
       const raptor_api_interfaces::msg::OutVirtuoseStatus::SharedPtr msg);
   void call_impedance_service();
-  void impedanceThread();
-  geometry_msgs::msg::TransformStamped getEndEffectorTransform();
-  void InitVFEnforcer();
+  void impedance_thread();
+  void get_ee_trans(geometry_msgs::msg::TransformStamped &trans);
+  void init_vf_enforcer();
   void project_target_on_sphere(Eigen::Vector3d &target_position_vec,
                                 double safety_sphere_radius_);
   geometry_msgs::msg::TransformStamped target_pose_tf_;

src/vf_control/include/system_interface.cpp

@@ -0,0 +1,15 @@
+#ifndef __SYSTEM_INTERFACE_HPP__
+#define __SYSTEM_INTERFACE_HPP__
+// this is the class that controls the haptic interface using its API,
+// you can redefine it to adapt the library to your device
+#include "raptor_api_interfaces/msg/in_virtuose_force.hpp"
+#include "raptor_api_interfaces/msg/in_virtuose_pose.hpp"
+#include "raptor_api_interfaces/msg/in_virtuose_speed.hpp"
+#include "raptor_api_interfaces/msg/out_virtuose_force.hpp"
+#include "raptor_api_interfaces/msg/out_virtuose_physical_pose.hpp"
+#include "raptor_api_interfaces/msg/out_virtuose_pose.hpp"
+#include "raptor_api_interfaces/msg/out_virtuose_speed.hpp"
+#include "raptor_api_interfaces/msg/out_virtuose_status.hpp"
+#include "raptor_api_interfaces/srv/virtuose_impedance.hpp"
+#include "raptor_api_interfaces/srv/virtuose_reset.hpp"
+#endif  // __SYSTEM_INTERFACE_HPP__

src/vf_control/include/utils.hpp

@@ -1,4 +1,4 @@
-geometry_msgs::msg::Point vector3ToPoint(
+geometry_msgs::msg::Point vector3_to_point(
     const geometry_msgs::msg::Vector3& vec) {
   geometry_msgs::msg::Point point;
   point.x = vec.x;

src/vf_control/include/closest_on_triangle.hpp → src/vf_control/include/vf/closest_on_triangle.hpp

@@ -3,7 +3,7 @@
 
 #include <Eigen/Dense>
 #include <cmath>
-#include "location.hpp"
+#include "vf/location.hpp"
 
 double round_up(const double value, const int decimal_places) {
   const double multiplier = std::pow(10.0, decimal_places);

src/vf_control/include/location.hpp → src/vf_control/include/vf/location.hpp

@@ -10,7 +10,7 @@ enum class Location {
   V1V3 = 5,
   V2V3 = 6
 };
-const inline char* LocationToString(Location l) {
+const inline char* location_to_string(Location l) {
   switch (l) {
     case Location::VOID:
       return "VOID";
@@ -32,7 +32,7 @@ const inline char* LocationToString(Location l) {
       return "INVALID";
   }
 };
-inline Location intToLocation(int i) {
+inline Location int_to_location(int i) {
   switch (i) {
     case -1:
       return Location::VOID;

src/vf_control/include/mesh.hpp → src/vf_control/include/vf/mesh.hpp

@@ -80,7 +80,7 @@ class Mesh {
       triangle_xfm_inv[i] = triangle_xfm[i].inverse();
     }
   }
-  void extrudeMeshRadially(const Eigen::Vector3d &origin, const double scale) {
+  void extrude_radially(const Eigen::Vector3d &origin, const double scale) {
     (void)origin;
     // find the faces with no neighbors
     std::vector<int> border_faces;
@@ -95,23 +95,26 @@ class Mesh {
 
       if (adjacency_dict.at({i, Location::V1V2}).size() == 0) {
         std::cout << "Extruding face " << i << " along V1V2" << std::endl;
-        extrudeEdgeAlongDirection(faces[i][0], faces[i][1], direction, scale);
+        extrude_edge_along_direction(faces[i][0], faces[i][1], direction,
+                                     scale);
       }
       if (adjacency_dict.at({i, Location::V2V3}).size() == 0) {
         std::cout << "Extruding face " << i << " along V2V3" << std::endl;
-        extrudeEdgeAlongDirection(faces[i][1], faces[i][2], direction, scale);
+        extrude_edge_along_direction(faces[i][1], faces[i][2], direction,
+                                     scale);
       }
       if (adjacency_dict.at({i, Location::V1V3}).size() == 0) {
         std::cout << "Extruding face " << i << " along V1V3" << std::endl;
-        extrudeEdgeAlongDirection(faces[i][0], faces[i][2], direction, scale);
+        extrude_edge_along_direction(faces[i][0], faces[i][2], direction,
+                                     scale);
       }
     }
     std::cout << "Face before extrusion: " << num_faces << std::endl;
     std::cout << "Face after extrusion: " << faces.size() << std::endl;
   }
-  void extrudeEdgeAlongDirection(const int v1, const int v2,
-                                 const Eigen::Vector3d &direction,
-                                 const double scale) {
+  void extrude_edge_along_direction(const int v1, const int v2,
+                                    const Eigen::Vector3d &direction,
+                                    const double scale) {
     // extrude the border faces
     const Eigen::Vector3d v1_ = vertices[v1];
     const Eigen::Vector3d v2_ = vertices[v2];

src/vf_control/include/vf_computation.hpp → src/vf_control/include/vf/vf_computation.hpp

@@ -6,18 +6,18 @@
 
 namespace compute_vf {
 template <typename Scalar>
-inline bool AlmostEqual(const Eigen::Matrix<Scalar, 3, 1> &vec1,
-                        const Eigen::Matrix<Scalar, 3, 1> &vec2,
-                        Scalar tolerance = static_cast<Scalar>(1e-6)) {
+inline bool almost_equal(const Eigen::Matrix<Scalar, 3, 1> &vec1,
+                         const Eigen::Matrix<Scalar, 3, 1> &vec2,
+                         Scalar tolerance = static_cast<Scalar>(1e-6)) {
   // Calculate the angle between the two vectors
   Scalar dotProduct = vec1.normalized().dot(vec2.normalized());
   Scalar angle = std::acos(dotProduct);
 
   // Calculate the axis difference (check if the vectors are almost parallel)
-  bool axisAlmostEqual =
+  bool axisalmost_equal =
       vec1.normalized().isApprox(vec2.normalized(), tolerance);
 
-  return ((angle <= tolerance) && axisAlmostEqual);
+  return ((angle <= tolerance) && axisalmost_equal);
 }
 
 Eigen::Vector3d enforce_virtual_fixture(
@@ -47,7 +47,7 @@ Eigen::Vector3d enforce_virtual_fixture(
   for (int Ti : T) {
     CP[Ti] = mesh.get_closest_on_triangle(current_position, Ti);
   }
-  // vis.DrawClosestPoints(CP, 0.002);
+  // vis.draw_closest_points(CP, 0.002);
   int iteration = 0;
   for (auto it = T.begin(); it != T.end();) {
     // std::cout << "-----\nIteration: " << iteration++ << std::endl;
@@ -66,11 +66,11 @@ Eigen::Vector3d enforce_virtual_fixture(
       if (cpi_loc == Location::V1 || cpi_loc == Location::V2 ||
           cpi_loc == Location::V3 || cpi_loc == Location::VOID) {
         // Constraint plane is the tangent plane to the sphere
-        if (AlmostEqual(current_position, CPi, eps)) {
+        if (almost_equal(current_position, CPi, eps)) {
           Eigen::Vector3d n = mesh.normals[*it].normalized();
           for (auto j = it + 1; j != T.end();) {
             auto [CPj, cpj_loc] = CP.at(*j);
-            if (AlmostEqual(CPi, CPj, eps) &&
+            if (almost_equal(CPi, CPj, eps) &&
                 mesh.is_locally_concave(*it, *j, cpi_loc)) {
               n += mesh.normals[*j].normalized();
               T.erase(j);
@@ -110,7 +110,7 @@ Eigen::Vector3d enforce_virtual_fixture(
 
           if (neighborIdxList1.size() > 0) {
             neighborIdx1 = neighborIdxList1[0];
-            if (AlmostEqual(CPi, CPn1, eps)) {
+            if (almost_equal(CPi, CPn1, eps)) {
               for (auto j = it + 1; j != T.end();) {
                 if (*j == neighborIdx1) {
                   T.erase(j);
@@ -125,7 +125,7 @@ Eigen::Vector3d enforce_virtual_fixture(
           }
           if (neighborIdxList2.size() > 0) {
             neighborIdx2 = neighborIdxList2[0];
-            if (AlmostEqual(CPi, CPn2, eps)) {
+            if (almost_equal(CPi, CPn2, eps)) {
               for (auto j = it + 1; j != T.end();) {
                 if (*j == neighborIdx2) {
                   T.erase(j);
@@ -157,8 +157,8 @@ Eigen::Vector3d enforce_virtual_fixture(
           }
           auto [CPia, cpia_loc] = CP.at(neighborIdx);
           if (mesh.is_locally_concave(Ti, neighborIdx, cpi_loc)) {
-            if (AlmostEqual(CPia, CPi, eps)) {
-              if (AlmostEqual(current_position, CPi, eps)) {
+            if (almost_equal(CPia, CPi, eps)) {
+              if (almost_equal(current_position, CPi, eps)) {
                 Eigen::Vector3d n =
                     (Ni + mesh.normals[neighborIdx].normalized()).normalized();
                 constraint_planes.push_back({n, CPi});

src/vf_control/include/vf_enforcer.hpp → src/vf_control/include/vf/vf_enforcer.hpp

@@ -50,9 +50,9 @@ class VFEnforcer {
     }
     open3d::io::WriteTriangleMesh(output_mesh_path_, *o3d_mesh);
     if (mesh_type_ == "file") {
-      visualizer_->AddPatientMesh(output_mesh_path_, skin_mesh_path_);
+      visualizer_->add_patient_mesh(output_mesh_path_, skin_mesh_path_);
     } else {
-      visualizer_->AddMesh(output_mesh_path_, 0);
+      visualizer_->add_mesh(output_mesh_path_, 0);
     }
     RCLCPP_INFO_STREAM(node->get_logger(),
                        "Loaded mesh with "
@@ -82,10 +82,10 @@ class VFEnforcer {
     this->x_des_old = x_des;
     this->delta_x_ << 0, 0, 0;
 
-    visualizer_->UpdateScene(constraint_planes_, x_des, x_old_, radius_);
+    visualizer_->update_scene(constraint_planes_, x_des, x_old_, radius_);
   }
 
-  Eigen::Vector3d EnforceVF(Eigen::Vector3d x_des) {
+  Eigen::Vector3d enforce_vf(Eigen::Vector3d x_des) {
     if ((x_des - x_des_old).norm() < 0.0005) {
       // return last delta_x_
       return Eigen::Vector3d(0, 0, 0);
@@ -103,7 +103,7 @@ class VFEnforcer {
     target_pose_vf.pose.position.y = x_new[1];
     target_pose_vf.pose.position.z = x_new[2];
     target_pose_vf.pose.orientation.w = 1.0;
-    visualizer_->UpdateScene(constraint_planes_, x_des, x_new, radius_);
+    visualizer_->update_scene(constraint_planes_, x_des, x_new, radius_);
     return delta_x_;
   }