Add experimental position logger

src/draw_svg/CMakeLists.txt

@@ -13,17 +13,20 @@ find_package(rclcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 
-#add_executable(draw_svg src/draw_svg.cpp)
-#target_include_directories(draw_svg PUBLIC
-#  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-#  $<INSTALL_INTERFACE:include>)
-#target_compile_features(draw_svg PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
-#ament_target_dependencies(
-#  draw_svg
-#  "ign_moveit2_examples"
-#)
-#install(TARGETS draw_svg
-#  DESTINATION lib/${PROJECT_NAME})
+# Install C++
+set(SRC_CPP_DIR src/cpp)
+# Example 0 - Follow target
+set(EXECUTABLE_0 log_position)
+add_executable(${EXECUTABLE_0} ${SRC_CPP_DIR}/${EXECUTABLE_0}.cpp)
+ament_target_dependencies(${EXECUTABLE_0}
+    rclcpp
+    geometry_msgs
+    moveit_ros_planning_interface
+)
+install(TARGETS
+    ${EXECUTABLE_0}
+    DESTINATION lib/${PROJECT_NAME}
+)
 
 # Install Python
 set(SRC_PY_DIR src/py)

src/draw_svg/launch/log_position.launch.py

@@ -0,0 +1,41 @@
+#!/usr/bin/env -S ros2 launch
+"""Launch Python example for following a target"""
+
+from os import path
+from typing import List
+
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+
+from ament_index_python import get_package_share_directory
+from launch.launch_description_sources import load_python_launch_file_as_module
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.event_handlers import OnProcessExit
+from launch.actions import OpaqueFunction
+
+
+def generate_launch_description() -> LaunchDescription:
+    log_position_node = Node(
+        package="draw_svg",
+        executable="log_position",
+        output='log',
+        arguments=[
+        ],
+    )
+    return LaunchDescription([log_position_node,])
+
+def generate_declared_arguments():
+    return [
+        OpaqueFunction(function=launch_setup)
+    ]

src/draw_svg/src/cpp/log_position.cpp

@@ -0,0 +1,76 @@
+#include <memory>
+#include <cstdio>
+#include <iostream>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+#include <moveit/move_group_interface/move_group_interface.h>
+//#include <moveit/moveit_commander>
+//https://github.com/AndrejOrsula/ign_moveit2_examples/blob/master/examples/cpp/ex_follow_target.cpp
+const std::string MOVE_GROUP = "lite6";
+
+class PositionLogger : public rclcpp::Node
+{
+public:
+  /// Constructor
+  PositionLogger();
+
+  /// Move group interface for the robot
+  moveit::planning_interface::MoveGroupInterface move_group_;
+  /// Subscriber for target pose
+  //rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr target_pose_sub_;
+  /// Target pose that is used to detect changes
+  //geometry_msgs::msg::Pose previous_target_pose_;
+
+private:
+  /// Callback that plans and executes trajectory each time the target pose is changed
+  void log_position();
+};
+
+PositionLogger::PositionLogger() : Node("position_logger"),
+                                   move_group_(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
+{
+  // Use upper joint velocity and acceleration limits
+  this->move_group_.setMaxAccelerationScalingFactor(1.0);
+  this->move_group_.setMaxVelocityScalingFactor(1.0);
+
+  // Subscribe to target pose
+  //target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
+  auto ros_clock = rclcpp::Clock::make_shared();
+  auto timer_ = rclcpp::create_timer(this, ros_clock, rclcpp::Duration::from_nanoseconds(5000000),
+   [=]()
+   {
+       this->log_position();
+   });
+
+  RCLCPP_INFO(this->get_logger(), "Initialization successful.");
+}
+
+void PositionLogger::log_position()
+{
+  auto const logger = rclcpp::get_logger("position_logger");
+
+  auto msg = this->move_group_.getCurrentPose();
+  auto p = msg.pose.position;
+  char a[100];
+  std::sprintf(a, "Position x: %f, y: %f, z: %f", p.x,p.y,p.z);
+  std::string s(a);
+
+  //rclcpp::RCLCPP_INFO(logger, a);
+  RCLCPP_INFO(logger, "AA");
+}
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  // TODO Try moveit_commander::roscpp_initialize
+
+  auto pl = std::make_shared<PositionLogger>();
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(pl);
+  executor.spin();
+
+  rclcpp::shutdown();
+  return EXIT_SUCCESS;
+}