cartesian motion: moved to advanced demos

Author: mhubii

lbr_demos/lbr_demos_fri_ros2_cpp/doc/cartesian space demo.pdf renamed to lbr_demos/lbr_demos_fri_ros2_advanced_cpp/doc/cartesian space demo.pdf

@@ -12,6 +12,7 @@
   <exec_depend>lbr_description</exec_depend>
 
   <depend>fri_vendor</depend>
+  <depend>geometry_msgs</depend>
   <depend>kdl_parser</depend>
   <depend>lbr_fri_ros2</depend>
   <depend>lbr_fri_msgs</depend>

lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml

@@ -25,7 +25,7 @@ class CartesianPosePublisherNode : public rclcpp::Node {
    * @function: callback function for Cartesian Pose Subscriber
    * @param msg Cartesian Pose of the robot
    */
-  void topic_callback(const geometry_msgs::msg::Pose &msg) {
+  void on_cartesian_pose(const geometry_msgs::msg::Pose &msg) {
     if (!is_init_) {
       initial_cartesian_pose_ = msg;
       is_init_ = true;
@@ -50,10 +50,10 @@ class CartesianPosePublisherNode : public rclcpp::Node {
     phase_ = 0.0;
 
     cartesian_pose_publisher_ =
-        this->create_publisher<geometry_msgs::msg::Pose>("/lbr/command/cartesian_pose", 10);
+        this->create_publisher<geometry_msgs::msg::Pose>("/lbr/command/cartesian_pose", 1);
     cartesian_pose_subscriber_ = this->create_subscription<geometry_msgs::msg::Pose>(
-        "/lbr/state/cartesian_pose", 10,
-        std::bind(&CartesianPosePublisherNode::topic_callback, this, _1));
+        "/lbr/state/cartesian_pose", 1,
+        std::bind(&CartesianPosePublisherNode::on_cartesian_pose, this, _1));
   }
 };
 

lbr_demos/lbr_demos_fri_ros2_cpp/src/cartesian_path_planning_node.cpp renamed to lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/cartesian_path_planning_node.cpp

@@ -1,13 +1,14 @@
 #include <string>
 
-#include "friClientIf.h"
 #include "geometry_msgs/msg/pose.hpp"
 #include "kdl/chainfksolverpos_recursive.hpp"
 #include "kdl/chainiksolverpos_lma.hpp"
 #include "kdl_parser/kdl_parser.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "friClientIf.h"
 #include "lbr_fri_msgs/msg/lbr_position_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
-#include "rclcpp/rclcpp.hpp"
 
 class CartesianPoseNode : public rclcpp::Node {
 protected:
@@ -77,15 +78,16 @@ class CartesianPoseNode : public rclcpp::Node {
 
     KDL::Tree robot_tree;
     if (!kdl_parser::treeFromString(robot_description_string, robot_tree)) {
-      std::cout << "Failed to construct kdl tree." << std::endl;
+      RCLCPP_ERROR(this->get_logger(), "Failed to construct kdl tree.");
+      return;
     }
 
     std::string root_link = "link_0"; // adjust with your URDF‘s root link
     std::string tip_link = "link_ee"; // adjust with your URDF‘s tip link
     if (!robot_tree.getChain(root_link, tip_link, chain_)) {
-      std::cerr << "Failed to get chain from tree." << std::endl;
+      RCLCPP_ERROR(this->get_logger(), "Failed to get chain from tree.");
     } else {
-      std::cout << "Get chain from tree successfully." << std::endl;
+      RCLCPP_INFO(this->get_logger(), "Get chain from tree successfully.");
     }
 
     lbr_position_command_pub_ = this->create_publisher<lbr_fri_msgs::msg::LBRPositionCommand>(

lbr_demos/lbr_demos_fri_ros2_cpp/src/cartesian_pose_node.cpp renamed to lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/cartesian_pose_node.cpp

@@ -15,51 +15,6 @@ 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_vendor 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_vendor
-  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
@@ -107,8 +62,6 @@ 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/CMakeLists.txt

@@ -12,12 +12,6 @@
   <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_vendor</depend>
-  <depend>kdl_parser</depend>
 
   <exec_depend>lbr_fri_ros2</exec_depend>
 
@@ -27,4 +21,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>