KDL based Movement in Cartesian Space

lbr_demos/lbr_demos_fri_ros2_cpp/CMakeLists.txt

@@ -15,6 +15,51 @@ find_package(fri_vendor REQUIRED)
 find_package(FRIClient REQUIRED)
 find_package(lbr_fri_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(urdf REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(orocos_kdl REQUIRED)
+find_package(kdl_parser REQUIRED)
+
+# cartesian_pose_node
+add_executable(cartesian_pose_node
+  src/cartesian_pose_node.cpp
+)
+
+ament_target_dependencies(cartesian_pose_node
+  fri_vendor
+  lbr_fri_msgs
+  rclcpp
+  tf2_ros
+  urdf
+  sensor_msgs
+  orocos_kdl
+  kdl_parser
+  geometry_msgs
+)
+
+target_link_libraries(cartesian_pose_node
+  FRIClient::FRIClient
+)
+
+# cartesian_path_planning_node
+add_executable(cartesian_path_planning_node
+  src/cartesian_path_planning_node.cpp
+)
+
+ament_target_dependencies(cartesian_path_planning_node
+  fri_vendor
+  lbr_fri_msgs
+  rclcpp
+  tf2_ros
+  sensor_msgs
+  geometry_msgs
+)
+
+target_link_libraries(cartesian_path_planning_node
+  FRIClient::FRIClient
+)
 
 # joint sine overlay
 add_executable(joint_sine_overlay_node
@@ -62,6 +107,8 @@ target_link_libraries(wrench_sine_overlay_node
 )
 
 install(TARGETS
+  cartesian_pose_node
+  cartesian_path_planning_node
   joint_sine_overlay_node
   torque_sine_overlay_node
   wrench_sine_overlay_node

lbr_demos/lbr_demos_fri_ros2_cpp/doc/KDL installation.pdf

 <depend>orocos_kdl_vendor</depend> 
 <depend>orocos_kdl_vendor</depend> 

lbr_demos/lbr_demos_fri_ros2_cpp/package.xml

@@ -12,6 +12,12 @@
   <depend>fri_vendor</depend>
   <depend>lbr_fri_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>tf2_ros</depend>
+  <depend>urdf</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>orocos_kdl</depend>
+  <depend>kdl_parser</depend>
 
   <exec_depend>lbr_fri_ros2</exec_depend>
 
@@ -21,4 +27,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

lbr_demos/lbr_demos_fri_ros2_cpp/src/cartesian_path_planning_node.cpp

@@ -0,0 +1,74 @@
+#include "rclcpp/rclcpp.hpp"
+#include "cmath"
+#include <iostream>
+#include <string>
+#include "geometry_msgs/msg/pose.hpp"
+
+using std::placeholders::_1;
+
+class CartesianPosePublisherNode:public rclcpp::Node
+{
+  private:
+    rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr cartesian_pose_publisher_;
+    rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr cartesian_pose_subscriber_;
+
+    geometry_msgs::msg::Pose initial_cartesian_pose_; // robot starts from this pose
+    bool is_init_; 
+
+    double amplitude_; // rad
+    double frequency_; // Hz
+    double sampling_time_; // sampling time for sending position command
+    double phase_; // initial phase
+
+
+  private:
+    /**
+     * @function: callback function for Cartesian Pose Subscriber
+     * @param msg Cartesian Pose of the robot
+    */
+    void topic_callback(const geometry_msgs::msg::Pose& msg)
+    {            
+      if(!is_init_) 
+      {
+        initial_cartesian_pose_ = msg;
+        is_init_ = true;
+      }
+      else
+      {
+        geometry_msgs::msg::Pose cartesian_pose_command = initial_cartesian_pose_;
+
+        phase_ = phase_ + 2 * M_PI * frequency_ * sampling_time_;
+        cartesian_pose_command.position.z += amplitude_ * sin(phase_);
+
+        cartesian_pose_publisher_->publish(cartesian_pose_command);
+      }
+
+      return;
+    }
+
+  public:
+    CartesianPosePublisherNode():Node("cartesian_pose_publisher_node")
+    {
+      is_init_ = false;
+      amplitude_ = 0.05;
+      frequency_ = 0.5;
+      sampling_time_ = 0.01;
+      phase_ = 0.0;
+
+      cartesian_pose_publisher_ = this->create_publisher<geometry_msgs::msg::Pose>(
+          "/lbr/command/cartesian_pose", 10);
+      cartesian_pose_subscriber_ = this->create_subscription<geometry_msgs::msg::Pose>(
+          "/lbr/state/cartesian_pose", 10, 
+          std::bind(&CartesianPosePublisherNode::topic_callback, this, _1));
+    }
+};
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::spin(std::make_shared<CartesianPosePublisherNode>()); 
+
+  rclcpp::shutdown(); 
+  return 0;
+}