diff --git a/CMakeLists.txt b/CMakeLists.txt
index 38342e7..a455864 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -30,11 +30,12 @@ find_package(geometry_msgs REQUIRED)
find_package(franka_cps_msgs REQUIRED)
find_package(franka_msgs REQUIRED)
-find_package(tf2)
-find_package(tf2_ros)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
add_executable(franka_iml_interface src/moveit_interface.cpp)
add_executable(franka_grasp_interface src/grasp_interface.cpp)
+add_executable(franka_pose_recorder src/pose_record.cpp)
ament_target_dependencies(
franka_iml_interface
@@ -66,12 +67,25 @@ ament_target_dependencies(
tf2_ros
)
+ament_target_dependencies(
+ franka_pose_recorder
+ rclcpp
+ geometry_msgs
+ tf2
+ tf2_ros
+)
+
install(
TARGETS franka_iml_interface
TARGETS franka_grasp_interface
+ TARGETS franka_pose_recorder
DESTINATION lib/${PROJECT_NAME}
)
+install(DIRECTORY
+ launch
+ DESTINATION share/${PROJECT_NAME}/)
+
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
diff --git a/config/panda.srdf b/config/panda.srdf
new file mode 100644
index 0000000..6433749
--- /dev/null
+++ b/config/panda.srdf
@@ -0,0 +1,120 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/pose_recoder.launch.py b/launch/pose_recoder.launch.py
new file mode 100755
index 0000000..1e329f4
--- /dev/null
+++ b/launch/pose_recoder.launch.py
@@ -0,0 +1,144 @@
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription,
+ Shutdown)
+from launch.conditions import UnlessCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+import yaml
+
+
+def load_yaml(package_name, file_path):
+ package_path = get_package_share_directory(package_name)
+ absolute_file_path = os.path.join(package_path, file_path)
+
+ try:
+ with open(absolute_file_path, 'r') as file:
+ return yaml.safe_load(file)
+ except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
+ return None
+
+
+def generate_launch_description():
+ robot_ip_parameter_name = 'robot_ip'
+ use_fake_hardware_parameter_name = 'use_fake_hardware'
+ fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+
+ robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+ use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+ fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+
+ # planning_context
+ franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots',
+ 'panda_arm.urdf.xacro')
+ robot_description_config = Command(
+ [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=true',
+ ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware,
+ ' fake_sensor_commands:=', fake_sensor_commands])
+
+ robot_description = {'robot_description': robot_description_config}
+
+ franka_semantic_xacro_file = os.path.join(get_package_share_directory('franka_moveit_config'),
+ 'srdf',
+ 'panda_arm.srdf.xacro')
+ # Publish TF
+ robot_state_publisher = Node(
+ package='robot_state_publisher',
+ executable='robot_state_publisher',
+ name='robot_state_publisher',
+ output='both',
+ parameters=[robot_description],
+ )
+
+ ros2_controllers_path = os.path.join(
+ get_package_share_directory('franka_moveit_config'),
+ 'config',
+ 'panda_ros_controllers.yaml',
+ )
+ ros2_control_node = Node(
+ package='controller_manager',
+ executable='ros2_control_node',
+ parameters=[robot_description, ros2_controllers_path],
+ remappings=[('joint_states', 'franka/joint_states')],
+ output={
+ 'stdout': 'screen',
+ 'stderr': 'screen',
+ },
+ on_exit=Shutdown(),
+ )
+
+ load_controllers = []
+ for controller in ['joint_state_broadcaster']:
+ load_controllers += [
+ ExecuteProcess(
+ cmd=['ros2 run controller_manager spawner {}'.format(controller)],
+ shell=True,
+ output='screen',
+ )
+ ]
+
+ joint_state_publisher = Node(
+ package='joint_state_publisher',
+ executable='joint_state_publisher',
+ name='joint_state_publisher',
+ parameters=[
+ {'source_list': ['franka/joint_states', 'panda_gripper/joint_states'], 'rate': 30}],
+ )
+
+ franka_robot_state_broadcaster = Node(
+ package='controller_manager',
+ executable='spawner',
+ arguments=['franka_robot_state_broadcaster'],
+ output='screen',
+ condition=UnlessCondition(use_fake_hardware),
+ )
+
+
+ franka_pose_recorder =Node(
+ package='franka_moveit_interface',
+ executable='franka_pose_recorder',
+ name='franka_pose_recorder',
+ output='screen',
+ parameters=[robot_description]
+ )
+
+
+ robot_arg = DeclareLaunchArgument(
+ robot_ip_parameter_name,
+ default_value='192.168.1.211',
+ description='Hostname or IP address of the robot.')
+
+ use_fake_hardware_arg = DeclareLaunchArgument(
+ use_fake_hardware_parameter_name,
+ default_value='false',
+ description='Use fake hardware')
+ fake_sensor_commands_arg = DeclareLaunchArgument(
+ fake_sensor_commands_parameter_name,
+ default_value='false',
+ description="Fake sensor commands. Only valid when '{}' is true".format(
+ use_fake_hardware_parameter_name))
+
+ gripper_launch_file = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([PathJoinSubstitution(
+ [FindPackageShare('franka_gripper'), 'launch', 'gripper.launch.py'])]),
+ launch_arguments={'robot_ip': robot_ip,
+ use_fake_hardware_parameter_name: use_fake_hardware}.items(),
+ )
+
+ return LaunchDescription(
+ [
+ robot_arg,
+ use_fake_hardware_arg,
+ fake_sensor_commands_arg,
+ robot_state_publisher,
+ joint_state_publisher,
+ franka_pose_recorder,
+ ros2_control_node,
+ franka_robot_state_broadcaster
+ ] + load_controllers
+ )
\ No newline at end of file
diff --git a/launch/run_grasp.launch.py b/launch/run_grasp.launch.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/grasp_interface.cpp b/src/grasp_interface.cpp
index eb21d95..afc49f4 100644
--- a/src/grasp_interface.cpp
+++ b/src/grasp_interface.cpp
@@ -9,8 +9,6 @@
#include
#include
-#include
-#include
#include
#include
@@ -18,18 +16,6 @@
#include
-#include "tf2_ros/transform_listener.h"
-
-#if __has_include()
-#include
-#else
-#include
-#endif
-#if __has_include()
-#include
-#else
-#include
-#endif
static const rclcpp::Logger LOGGER = rclcpp::get_logger("franka_grasp_interface");
namespace mtc = moveit::task_constructor;
@@ -39,11 +25,9 @@ class GraspInterfaceNode : public rclcpp::Node
public:
using GraspSequence = franka_cps_msgs::action::GraspSequence;
using GoalHandleGraspSequence = rclcpp_action::ServerGoalHandle;
- using FrankaMove = franka_msgs::action::Move;
- using FrankaGrasp = franka_msgs::action::Grasp;
GraspInterfaceNode()
- : Node("grasp_interface"), tf_buffer_(this->get_clock()),tf_listener_(tf_buffer_)
+ : Node("grasp_interface")
{
// Action server for getting new Grasp Sequences
action_server_ = rclcpp_action::create_server(
@@ -53,60 +37,26 @@ class GraspInterfaceNode : public rclcpp::Node
std::bind(&GraspInterfaceNode::handle_cancel, this, std::placeholders::_1),
std::bind(&GraspInterfaceNode::handle_accepted, this, std::placeholders::_1));
- // Publisher for pose
- pose_publisher_ = this->create_publisher("franka_grasp_moveit/current_pose", 10);
-
- // Service for IK
-
-
- // TF2
- tf2_ros::Buffer tf_buffer_(this->get_clock());
- tf2_ros::TransformListener tf_listener_(tf_buffer_);
- tf2_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&GraspInterfaceNode::compute_endeffector_pose, this));
-
// Setup for move groups
static const std::string arm_group_name = "panda_arm";
- static const std::string hand_group_name = "hand";
- static const std::string hand_frame = "panda_hand";
+ static const std::string hand_group_name = "hand";
+ static const std::string hand_frame = "panda_link8";
+ static const std::string eef_name = "hand";
+
+
}
private:
rclcpp_action::Server::SharedPtr action_server_;
- rclcpp::Publisher::SharedPtr pose_publisher_;
- tf2_ros::Buffer tf_buffer_;
- tf2_ros::TransformListener tf_listener_;
- rclcpp::TimerBase::SharedPtr tf2_timer_;
-
- void compute_endeffector_pose()
- {
- try
- {
- geometry_msgs::msg::TransformStamped transform_stamped;
- transform_stamped = tf_buffer_.lookupTransform("panda_link1", "panda_link7", rclcpp::Time(0));
-
- geometry_msgs::msg::PoseStamped current_pose;
- current_pose.header.frame_id = "panda_link8";
- current_pose.header.stamp = transform_stamped.header.stamp;
- //Position
- current_pose.pose.position.x = transform_stamped.transform.translation.x;
- current_pose.pose.position.y = transform_stamped.transform.translation.y;
- current_pose.pose.position.z = transform_stamped.transform.translation.z;
- // Orientation
- current_pose.pose.orientation.x = transform_stamped.transform.rotation.x;
- current_pose.pose.orientation.y = transform_stamped.transform.rotation.y;
- current_pose.pose.orientation.z = transform_stamped.transform.rotation.z;
- current_pose.pose.orientation.w = transform_stamped.transform.rotation.w;
- pose_publisher_->publish(current_pose);
- }
- catch (tf2::TransformException &ex)
- {
- RCLCPP_WARN(this->get_logger(), "%s", ex.what());
- }
- }
+ static const std::string arm_group_name;
+ static const std::string hand_group_name;
+ static const std::string hand_frame;
+ static const std::string eef_name;
+ const std::array tcp_frame = {0, 0, 0.1034, 0, 0, 0};
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
- std::shared_ptr goal)
+ std::shared_ptr /*goal*/)
{
RCLCPP_INFO(this->get_logger(), "Received goal request!");
(void)uuid;
@@ -157,68 +107,152 @@ class GraspInterfaceNode : public rclcpp::Node
task.loadRobotModel(this->shared_from_this());
// set Planners
+ auto sampling_planner = std::make_shared(this->shared_from_this());
auto interpolation_planner = std::make_shared();
+ auto cartesian_planner = std::make_shared();
+ cartesian_planner->setMaxVelocityScalingFactor(1.0);
+ cartesian_planner->setMaxAccelerationScalingFactor(1.0);
+ cartesian_planner->setStepSize(.01);
- std::vector joint_positions;
- /*
- // Compute IK for each pose and create corresponding stages
- if (computeIK(goal->hover, joint_positions)) {
- // Create and add MoveTo stage using computed joint positions
- auto move_to_hover = std::make_unique("Move to Hover", interpolation_planner);
- move_to_hover->setGroup(arm_group_name); // set group
- move_to_hover->setGoal(joint_positions); // set goal
- task.add(std::move(move_to_hover)); // add Task
- } else {
- RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for hover pose");
- }
+ // Set task properties
+ task.setProperty("group", arm_group_name);
+ task.setProperty("eef", eef_name);
+ task.setProperty("hand", hand_group_name);
+ task.setProperty("hand_grasping_frame", hand_frame);
+ task.setProperty("ik_frame", hand_frame);
- if (computeIK(goal->grasp, joint_positions)) {
- auto move_to_hover = std::make_unique("Move to Hover", interpolation_planner);
- move_to_hover->setGroup(arm_group_name); // set group
- move_to_hover->setGoal(joint_positions); // set goal
- task.add(std::move(move_to_hover)); // add Task
- } else {
- RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for hover pose");
- }
+ // Current State
+ auto current_state = std::make_unique("current state");
- if (computeIK(goal->hover, joint_positions)) {
- // Create and add MoveTo stage using computed joint positions
- auto move_to_hover2 = std::make_unique("Move to Hover", interpolation_planner);
- move_to_hover2->setGroup(arm_group_name); // set group
- move_to_hover2->setGoal(joint_positions); // set goal
- task.add(std::move(move_to_hover2)); // add Task
- } else {
- RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for hover pose");
- }
+ // Open Hand
+ moveit::task_constructor::Stage* initial_state_ptr = nullptr;
- if (computeIK(goal->drop, joint_positions)) {
- // Create and add MoveTo stage using computed joint positions
- auto move_to_drop = std::make_unique("Move to drop", interpolation_planner);
- move_to_drop->setGroup(arm_group_name); // set group
- move_to_drop->setGoal(joint_positions); // set goal
- task.add(std::move(move_to_drop)); // add Task
- } else {
- RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for drop pose");
- }
+ auto stage1 = std::make_unique("open hand", sampling_planner);
+ stage1.setGroup(hand_group_name);
+ stage1.setGoal("open");
+ initial_state_ptr = stage1.get();
+ task.add(std::move(stage1));
- if (computeIK(goal->idle, joint_positions)) {
- // Create and add MoveTo stage using computed joint positions
- auto move_to_idle = std::make_unique("Move to idle", interpolation_planner);
- move_to_idle->setGroup(arm_group_name); // set group
- move_to_idle->setGoal(joint_positions); // set goal
- task.add(std::move(move_to_idle)); // add Task
- } else {
- RCLCPP_ERROR(this->get_logger(), "Failed to compute IK for idle pose");
- }
- */
+
+
+ // Grasping move
+ auto grasp = std::make_unique("pick object");
+ task.properties().exposeTo(grasp.properties(), { "eef", "hand", "group", "ik_frame" });
+ grasp.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
+
+ // Move Hower
+ auto stage2 = std::make_unique("generate hower pose");
+ stage2.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
+ stage2.properties().set("marker_ns", "hower_pose");
+ stage2.setPose(goal.hower)
+ stage2.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
+
+ // Compute IK Hower
+ auto wrapper1 = std::make_unique("hower pose IK", std::move(stage));
+ wrapper1.setMaxIKSolutions(2);
+ wrapper1.setMinSolutionDistance(1.0);
+ wrapper1.setIKFrame(vectorToEigen(tcp_frame), hand_frame);
+ wrapper1.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
+ wrapper1.properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
+ grasp.insert(std::move(wrapper1));
+
+ // Move Grasp
+ auto stage3 = std::make_unique("generate grasp pose");
+ stage3.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
+ stage3.properties().set("marker_ns", "grasp_pose");
+ stage3.setPose(goal.grasp)
+ stage3.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
+
+ // Compute IK Grasp
+ auto wrapper2 = std::make_unique("grasp pose IK", std::move(stage));
+ wrapper2.setMaxIKSolutions(2);
+ wrapper2.setMinSolutionDistance(1.0);
+ wrapper2.setIKFrame(vectorToEigen(tcp_frame), hand_frame);
+ wrapper2.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
+ wrapper2.properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
+ grasp.insert(std::move(wrapper2));
+
+ // Allow Collision
+ auto stage4 = std::make_unique("allow collision (hand,object)");
+ stage4.allowCollisions(
+ "object",
+ task.getRobotModel().getJointModelGroup(hand_group_name).getLinkModelNamesWithCollisionGeometry(),
+ true);
+ grasp.insert(std::move(stage4));
+
+ // Close Hand
+ auto stage5 = std::make_unique("close hand", sampling_planner);
+ stage5.setGroup(hand_group_name);
+ stage5.setGoal("close");
+ grasp.insert(std::move(stage5));
+
+ // Move Back to Hower
+ auto stage6 = std::make_unique("generate hower pose");
+ stage6.properties().configureInitFrom(Stage::PARENT);
+ stage6.properties().set("marker_ns", "hower_pose");
+ stage6.setPose(goal.hower)
+ stage6.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
+
+ // Compute IK Hower
+ grasp.insert(std::move(wrapper2));
+
+ // Transition to Drop Phase
+ task.add(std::move(grasp));
+
+
+ // Move Drop
+ auto stage7 = std::make_unique("generate hower pose");
+ stage7.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
+ stage7.properties().set("marker_ns", "hower_pose");
+ stage7.setPose(goal.drop)
+ stage7.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
+
+ // Compute IK Drop
+ auto wrapper3 = std::make_unique("drop pose IK", std::move(stage));
+ wrapper3.setMaxIKSolutions(2);
+ wrapper3.setMinSolutionDistance(1.0);
+ wrapper3.setIKFrame(vectorToEigen(tcp_frame), hand_frame);
+ wrapper3.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
+ wrapper3.properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
+ task.add(std::move(wrapper3));
+
+ // Open Hand
+ auto stage8 = std::make_unique("open hand", sampling_planner);
+ stage8.setGroup(hand_group_name);
+ stage8.setGoal("open");
+ task.add(std::move(stage8));
+
+ // Move Idle
+ auto stage9 = std::make_unique("generate idle pose");
+ stage9.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
+ stage9.properties().set("marker_ns", "hower_pose");
+ stage9.setPose(goal.idle)
+ stage9.setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions
+
+ // Compute IK Idle
+ auto wrapper4 = std::make_unique("drop pose IK", std::move(stage));
+ wrapper4.setMaxIKSolutions(2);
+ wrapper4.setMinSolutionDistance(1.0);
+ wrapper4.setIKFrame(vectorToEigen(tcp_frame), hand_frame);
+ wrapper4.properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
+ wrapper4.properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
+ task.add(std::move(wrapper4));
return task;
}
- bool executeTask(moveit::task_constructor::Task& task) {
- // Logic to execute the given task
- // Return true if successful, false otherwise
- return 1;
+ bool executeTask(moveit::task_constructor::Task::Task task) {
+ RCLCPP_INFO(LOGGER, "Executing solution trajectory");
+ moveit_msgs::msg::MoveItErrorCodes execute_result;
+
+ execute_result = task.execute(*task.solutions().front());
+
+ if (execute_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
+ RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed and returned: " << execute_result.val);
+ return false;
+ }
+
+ return true;
}
};
diff --git a/src/moveit_interface.cpp b/src/moveit_interface.cpp
index 147b7ff..cda2f2d 100644
--- a/src/moveit_interface.cpp
+++ b/src/moveit_interface.cpp
@@ -22,6 +22,7 @@
#include "moveit/planning_scene_interface/planning_scene_interface.h"
#include "tf2_ros/transform_listener.h"
+#include
using namespace std::chrono_literals;
diff --git a/src/pose_record.cpp b/src/pose_record.cpp
new file mode 100644
index 0000000..7571bcd
--- /dev/null
+++ b/src/pose_record.cpp
@@ -0,0 +1,77 @@
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+#include "geometry_msgs/msg/transform_stamped.h"
+
+#include "tf2_ros/transform_listener.h"
+#include
+
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("franka_pose_recorder");
+using namespace std::chrono_literals;
+
+class PoseRecoderNode : public rclcpp::Node
+{
+ public:
+ using PoseStampedMsg = geometry_msgs::msg::PoseStamped;
+
+ PoseRecoderNode()
+ : Node("pose_recoder"), tf_buffer_(this->get_clock()),tf_listener_(tf_buffer_)
+ {
+ // Publisher for pose
+ pose_publisher_ = this->create_publisher("pose_recoder/current_pose", 10);
+
+
+ // TF2
+ tf2_ros::Buffer tf_buffer_{this->get_clock()};
+ tf2_ros::TransformListener tf_listener_{tf_buffer_};
+ tf2_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&PoseRecoderNode::compute_endeffector_pose, this));
+
+ }
+
+ private:
+ rclcpp::Publisher::SharedPtr pose_publisher_;
+ tf2_ros::Buffer tf_buffer_;
+ tf2_ros::TransformListener tf_listener_;
+ rclcpp::TimerBase::SharedPtr tf2_timer_;
+
+ void compute_endeffector_pose()
+ {
+ try
+ {
+ geometry_msgs::msg::TransformStamped transform_stamped;
+ transform_stamped = tf_buffer_.lookupTransform("panda_link1", "panda_link7", rclcpp::Time(0));
+
+ geometry_msgs::msg::PoseStamped current_pose;
+ current_pose.header.frame_id = "panda_link8";
+ current_pose.header.stamp = transform_stamped.header.stamp;
+ //Position
+ current_pose.pose.position.x = transform_stamped.transform.translation.x;
+ current_pose.pose.position.y = transform_stamped.transform.translation.y;
+ current_pose.pose.position.z = transform_stamped.transform.translation.z;
+ // Orientation
+ current_pose.pose.orientation.x = transform_stamped.transform.rotation.x;
+ current_pose.pose.orientation.y = transform_stamped.transform.rotation.y;
+ current_pose.pose.orientation.z = transform_stamped.transform.rotation.z;
+ current_pose.pose.orientation.w = transform_stamped.transform.rotation.w;
+ pose_publisher_->publish(current_pose);
+ }
+ catch (tf2::TransformException &ex)
+ {
+ RCLCPP_WARN(this->get_logger(), "%s", ex.what());
+ }
+ }
+};
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}