test scene for pick and place

rasmt_moveit_config/launch/rasmt_moveit.launch.py

@@ -174,29 +174,29 @@ def generate_launch_description():
         ]
     )
 
-    move_to_joint_state_action_server = Node(
+    # move_to_joint_state_action_server = Node(
-        package="robossembler_servers",
+    #     package="robossembler_servers",
-        executable="move_to_joint_states_action_server",
+    #     executable="move_to_joint_states_action_server",
-        name="joint_states_moveit",
+    #     name="joint_states_moveit",
-        parameters=[
+    #     parameters=[
-            robot_description,
+    #         robot_description,
-            robot_description_semantic,
+    #         robot_description_semantic,
-            kinematics_yaml,
+    #         kinematics_yaml,
-            robot_description_planning,
+    #         robot_description_planning,
-            ompl_yaml,
+    #         ompl_yaml,
-            planning,
+    #         planning,
-            trajectory_execution,
+    #         trajectory_execution,
-            moveit_controllers,
+    #         moveit_controllers,
-            planning_scene_monitor_parameters,
+    #         planning_scene_monitor_parameters,
-            use_sim_time
+    #         use_sim_time
-        ]
+    #     ]
-    )
+    # )
 
     launch_nodes = []
     launch_nodes.append(rviz)
     launch_nodes.append(move_group_node)
     launch_nodes.append(move_topose_action_server)
-    launch_nodes.append(move_to_joint_state_action_server)
+   # launch_nodes.append(move_to_joint_state_action_server)
 
 
 

rasmt_support/config/rasmt_ros2_controllers.yaml

@@ -6,7 +6,7 @@ controller_manager:
       type: joint_trajectory_controller/JointTrajectoryController
 
     rasmt_hand_controller:
-      type: effort_controllers/JointGroupEffortController
+      type: forward_command_controller/ForwardCommandController
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
@@ -32,9 +32,4 @@ rasmt_hand_controller:
     joints: 
       - rasmt_Slide_1
       - rasmt_Slide_2
-    command_interfaces:
+    interface_name: position
-      - effort
-    state_interfaces:
-      - position
-      - velocity
-      - effort

rasmt_support/launch/rasmt_control.launch.py

@@ -67,6 +67,10 @@ def generate_launch_description():
         executable="spawner.py",
         arguments=["rasmt_hand_controller", "--controller-manager", "/controller_manager"],
     )
+    action_server_controller_hand_node = Node(
+        package="robossembler_servers",
+        executable="gripper_cmd_node"
+    )
 
     return LaunchDescription(
         launch_args + [
@@ -74,5 +78,6 @@ def generate_launch_description():
             robot_state_publisher,
             joint_state_broadcaster,
             controller_arm,
-            controller_hand
+            controller_hand,
+            action_server_controller_hand_node
     ])

rasmt_support/urdf/tools/rasmt_hand_control.xacro

@@ -17,9 +17,9 @@
             </xacro:unless>
 
             <joint name="${prefix}_Slide_1">
-                <command_interface name="effort">
+                <command_interface name="position">
                     <!-- better to use radians as min max first -->
-                    <param name="min">-10</param>
+                    <param name="min">0</param>
                     <param name="max">10</param>
                 </command_interface>
                 <state_interface name="position"/>
@@ -30,8 +30,8 @@
             <joint name="${prefix}_Slide_2">
                 <!-- <param name="mimic">${prefix}_Slide_1</param>
                 <param name="multiplier">1</param> -->
-                <command_interface name="effort">
+                <command_interface name="position">
-                    <param name="min">-10</param>
+                    <param name="min">0</param>
                     <param name="max">10</param>
                 </command_interface>
                 <state_interface name="position"/>

rasmt_support/urdf/tools/rasmt_hand_macro.xacro

@@ -52,7 +52,7 @@
     type="fixed">
     <origin
       xyz="0 0.0 0.12324"
-      rpy="-3.14159265358979 0 -1.5707963267949" />
+      rpy="-3.14159265358979 0 0" />
     <parent
       link="${prefix}_Dock_Link" />
     <child

robossembler/CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(plansys2_bt_actions REQUIRED)
 find_package(gazebo_msgs REQUIRED)
 find_package(robossembler_interfaces REQUIRED)
 find_package(behavior_tree REQUIRED)
+find_package(control_msgs REQUIRED)
 
 if (NOT CMAKE_CXX_STANDARD)
     set(CMAKE_CXX_STANDARD 17)
@@ -46,6 +47,7 @@ set(dependencies
     robossembler_interfaces
     behavior_tree
     std_msgs
+    control_msgs
 )
 
 include_directories(include)

robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml

@@ -5,8 +5,11 @@
         <Sequence>
             <Action ID="Print" frame="${arg2}" server_name="/spawn_entity" server_timeout="10"/>
             <Action ID="GetEntityState" part_name="${arg2}" server_name="/get_entity_state" server_timeout="10"/>
-            <Action ID="MoveToPose" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
+            <Action ID="MoveToPose" command="go_to" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
-            <Action ID="MoveGripper" arm_group="${arg1}" server_name="/move_to_joint_states" server_timeout="10" cancel_timeout="5" />
+            <Action ID="GripperCmd" command="open" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
+            <Action ID="MoveToPose" command="pick" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
+            <Action ID="GripperCmd" command="close" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
+            <Action ID="MoveToPose" command="go_to" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
         </Sequence>
     </BehaviorTree>
     <!-- ////////// -->
@@ -16,9 +19,11 @@
         </Action>
         <Action ID="MoveToPose">
             <input_port name="arm_group"/>
+            <input_port name="command"/>
         </Action>
         <Action ID="MoveGripper">
             <input_port name="arm_group"/>
+            <input_port name="command"/>
         </Action>
         <Action ID="Print">
             <input_port name="frame"/>

robossembler/config/params.yaml

@@ -15,3 +15,10 @@ assemble_1:
       printer2: [-1.0, -1.0, 0.0, 
                  0.0, 0.0, 0.0,
                  0.0]
+    pick_entity:
+        go_to: [0.40644, 0.0, 0.3274, 0.0, 1.0, 0.0, 0.009]
+        bring: [0.40644, 0.0, 0.2274, 0.0, 1.0, 0.0, 0.009]
+    gripp_cmd:
+        open: 0.06
+        close: 0.02
+

robossembler/include/robot_bt/behavior_tree_nodes/Print.hpp

@@ -9,17 +9,16 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-using SpawnEntitySrv = gazebo_msgs::srv::SpawnEntity;
 
-class Print : public BtService<SpawnEntitySrv>
+class Print : public BtService<gazebo_msgs::srv::SpawnEntity>
 {
 public:
     Print(const std::string &xml_tag_name, 
         const BT::NodeConfiguration &conf);
 
-    SpawnEntitySrv::Request::SharedPtr populate_request();
+    gazebo_msgs::srv::SpawnEntity::Request::SharedPtr populate_request();
 
-    BT::NodeStatus handle_response(SpawnEntitySrv::Response::SharedPtr response);
+    BT::NodeStatus handle_response(gazebo_msgs::srv::SpawnEntity::Response::SharedPtr response);
 
     static BT::PortsList providedPorts()
     {

robossembler/src/behavior_tree_nodes/Print.cpp

@@ -9,6 +9,8 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include "robot_bt/behavior_tree_nodes/Print.hpp"
 
+using SpawnEntitySrv = gazebo_msgs::srv::SpawnEntity;
+
 Print::Print(const std::string &xml_tag_name,
                 const BT::NodeConfiguration &conf)
 :BtService<SpawnEntitySrv>(xml_tag_name, conf)
@@ -18,8 +20,8 @@ Print::Print(const std::string &xml_tag_name,
     a.position.y = 0.8;
     a.position.z = 0.0;
     geometry_msgs::msg::Pose b;
-    a.position.x = -0.5;
+    a.position.x = 0.4;
-    a.position.y = -0.5;
+    a.position.y = 0.0;
     a.position.z =  0.05;
     printer_coords_.insert(std::make_pair("printerB", b));
     printer_coords_.insert(std::make_pair("printerA", a));

robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp

@@ -28,7 +28,7 @@ class GetEntityState : public BtService<GetEntityStateSrv>
         }
         BT::NodeStatus handle_response(GetEntityStateSrv::Response::SharedPtr response) override
         {
-            RCLCPP_INFO(_node->get_logger(), "Service call complete");
+            RCLCPP_INFO_STREAM(_node->get_logger(), "part_name ["<< part_name_ <<"] Entity state x = "<< response->state.pose.position.x <<" y = "<< response->state.pose.position.y <<" z = "<< response->state.pose.position.z <<"");
             return BT::NodeStatus::SUCCESS;
         }
 

robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp

@@ -1,33 +1,34 @@
-#include "robossembler_interfaces/action/moveit_send_joint_states.hpp"
+//#include "robossembler_interfaces/action/moveit_send_joint_states.hpp"
+//#include "control_msgs/action/follow_joint_trajectory.hpp"
+#include "robossembler_interfaces/action/gripper_command.hpp"
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "behavior_tree/BtAction.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "moveit_msgs/action/move_group.h"
 
 using namespace BT;
-using MoveToJointStateAction = robossembler_interfaces::action::MoveitSendJointStates;
+using GripperCmd = robossembler_interfaces::action::GripperCommand;
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_to_joitn_state_action_client");
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("gripper_cmd");
 
-class MoveToJointState : public BtAction<MoveToJointStateAction>
+class SendGripperCmd : public BtAction<GripperCmd>
 {
     public:
-        MoveToJointState(const std::string & name, const BT::NodeConfiguration & config) 
+        SendGripperCmd(const std::string & name, const BT::NodeConfiguration & config) 
-        : BtAction<MoveToJointStateAction>(name, config) {
+        : BtAction<GripperCmd>(name, config) {
-            gripper_gap = 0.02;
+            //gripper_gap = 0.02;
+            gripper_point_a_ = 0.06;
+            gripper_point_b_ = 0.025;
+
+            gripper_cmd.insert(std::make_pair("open", gripper_point_a_));
+            gripper_cmd.insert(std::make_pair("close", gripper_point_b_));
+
         }
 
-        MoveToJointStateAction::Goal populate_goal() override
+        GripperCmd::Goal populate_goal() override
         {
-            getInput<std::string>("arm_group", robot_name_);
-            //robot_name_ = getInput<std::string>("arm_group").value();
 
-            RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
+            auto goal = GripperCmd::Goal();
-            auto goal = MoveToJointStateAction::Goal();
+            getInput<std::string>("command", command);
-            goal.robot_name = robot_name_;
+            goal.value = gripper_cmd[command];
-            goal.joints_acceleration_scaling_factor = 0.1;
+            //goal.trajectory.points = gripper_cmd[command];
-            goal.joints_velocity_scaling_factor = 0.1;
-            goal.joint_value = gripper_gap;
-            RCLCPP_INFO(_node->get_logger(), "Goal populated");
 
             return goal;
         }
@@ -35,16 +36,19 @@ class MoveToJointState : public BtAction<MoveToJointStateAction>
         static PortsList providedPorts()
         {
             return providedBasicPorts({
-                    InputPort<std::string>("arm_group")});
+                    InputPort<std::string>("command")});
         }
 
     private:
-        double gripper_gap{0};
+        std::vector<std::string> joint_names_ = {"rasmt_Slide_1", "rasmt_Slide_2"};
-        std::string robot_name_;
+        double gripper_point_a_{0.0};
+        double gripper_point_b_{0.0};
+        std::map<std::string, double> gripper_cmd;
+        std::string command;
 
 }; // class MoveToJointState
 
 BT_REGISTER_NODES(factory)
 {
-    factory.registerNodeType<MoveToJointState>("MoveGripper");
+    factory.registerNodeType<SendGripperCmd>("GripperCmd");
 }

robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp

@@ -3,6 +3,8 @@
 #include "behavior_tree/BtAction.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "moveit_msgs/action/move_group.h"
+#include "Eigen/Geometry"
+#include "rclcpp/parameter.hpp"
 
 using namespace BT;
 using MoveToPoseAction = robossembler_interfaces::action::MoveitSendPose;
@@ -13,26 +15,39 @@ class MoveToPose : public BtAction<MoveToPoseAction>
     public:
         MoveToPose(const std::string & name, const BT::NodeConfiguration & config) 
         : BtAction<MoveToPoseAction>(name, config) {
-            target_pose_.position.x = 0.2;
+            target_pose_a_.position.x = 0.40644;
-            target_pose_.position.y = 0.2;
+            target_pose_a_.position.y = 0.0;
-            target_pose_.position.z = 0.7;
+            target_pose_a_.position.z = 0.3274;
-            //target_pose_.orientation.x = 0;
+            target_pose_a_.orientation.x = 0;
-            //target_pose_.orientation.y = 0.9764;
+            target_pose_a_.orientation.y = 1.0;
-            //target_pose_.orientation.z = 0;
+            target_pose_a_.orientation.z = 0.0;
-            //target_pose_.orientation.w = 0.2160;
+            target_pose_a_.orientation.w = 0.009;
+
+            target_pose_b_.position.x = 0.40644;
+            target_pose_b_.position.y = 0.0;
+            target_pose_b_.position.z = 0.20;
+            target_pose_b_.orientation.x = 0;
+            target_pose_b_.orientation.y = 1.0;
+            target_pose_b_.orientation.z = 0.0;
+            target_pose_b_.orientation.w = 0.009;
+            
+            pick_and_place_command.insert(std::make_pair("go_to", target_pose_a_));
+            pick_and_place_command.insert(std::make_pair("pick", target_pose_b_));
+
+
         }
 
         MoveToPoseAction::Goal populate_goal() override
         {
             getInput<std::string>("arm_group", robot_name_);
             //robot_name_ = getInput<std::string>("arm_group").value();
-
+            getInput<std::string>("command", command);
             RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
             auto goal = MoveToPoseAction::Goal();
             goal.robot_name = robot_name_;
             goal.end_effector_acceleration = 1.0;
             goal.end_effector_velocity = 1.0;
-            goal.target_pose = target_pose_;
+            goal.target_pose = pick_and_place_command[command];
             RCLCPP_INFO(_node->get_logger(), "Goal populated");
 
             return goal;
@@ -41,12 +56,17 @@ class MoveToPose : public BtAction<MoveToPoseAction>
         static PortsList providedPorts()
         {
             return providedBasicPorts({
-                    InputPort<std::string>("arm_group")});
+                    InputPort<std::string>("arm_group"),
+                    InputPort<std::string>("command")});
         }
 
     private:
-        geometry_msgs::msg::Pose target_pose_;
+        geometry_msgs::msg::Pose target_pose_a_;
+        geometry_msgs::msg::Pose target_pose_b_;
         std::string robot_name_;
+        std::map<std::string, geometry_msgs::msg::Pose> pick_and_place_command;
+        std::string command;
+        std::string pick;
 
 }; // class MoveToPose
 

robossembler_interfaces/CMakeLists.txt

@@ -23,10 +23,10 @@ find_package(geometry_msgs REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
     "action/MoveitSendPose.action"
     "action/MoveitSendJointStates.action"
+    "action/GripperCommand.action"
     "msg/PropertyValuePair.msg"
     "msg/ActionFeedbackStatusConstants.msg"
     "msg/ActionResultStatusConstants.msg"
-    "srv/GripperCommand.srv"
     DEPENDENCIES geometry_msgs std_msgs
 )
 

robossembler_interfaces/action/GripperCommand.action

@@ -1,3 +1,4 @@
 float64 value
 ---
-string success
+bool success
+---

robossembler_servers/CMakeLists.txt

@@ -53,21 +53,18 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-
+add_library(gripper_cmd_action_server SHARED src/gripper_cmd.cpp)
-add_library(gripper_cmd_srv_lib SHARED src/gripper_cmd_srv.cpp)
+ament_target_dependencies(gripper_cmd_action_server ${deps})
-target_compile_definitions(gripper_cmd_srv_lib PRIVATE "GRIPPER_CMD_BUILDING_DLL")
+target_include_directories(gripper_cmd_action_server PRIVATE
-ament_target_dependencies(gripper_cmd_srv_lib ${deps})
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_DIR}/include>
-
+    $<INSTALL_INTERFACE:include>)
-rclcpp_components_register_node(gripper_cmd_srv_lib
+target_compile_definitions(gripper_cmd_action_server PRIVATE "GRIPPER_CMD_ACTION_SERVER_CPP_BUILDING_DLL")
-  PLUGIN "GripperCmd"
+rclcpp_components_register_node(gripper_cmd_action_server PLUGIN "robossembler_servers::GripperCmd" EXECUTABLE gripper_cmd_node)
-  EXECUTABLE gripper_cmd_srv)
 
 add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp)
-# target_include_directories(move_to_joint_states_action_server PRIVATE include)
 ament_target_dependencies(move_to_joint_states_action_server ${deps})
 
 add_executable(move_topose_action_server src/move_topose_action_server.cpp)
-# target_include_directories(move_topose_action_server PRIVATE include)
 ament_target_dependencies(move_topose_action_server ${deps})
 
 
@@ -75,7 +72,7 @@ install(
   TARGETS 
   move_topose_action_server
   move_to_joint_states_action_server
-  gripper_cmd_srv_lib
+  gripper_cmd_action_server
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}

robossembler_servers/src/gripper_cmd.cpp

@@ -0,0 +1,95 @@
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
+#include "robossembler_interfaces/action/gripper_command.hpp"
+
+using GripperCmdAction = robossembler_interfaces::action::GripperCommand;
+
+namespace robossembler_servers
+{
+class GripperCmd : public rclcpp::Node
+{
+public:
+
+    explicit GripperCmd(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) 
+    : Node("gripper_cmd", options)
+    {
+        using namespace std::placeholders;
+        this->action_server_ = rclcpp_action::create_server<GripperCmdAction>(
+            this,
+            "/gripper_cmd",
+            std::bind(&GripperCmd::goal_callback, this, _1, _2),
+            std::bind(&GripperCmd::cancel_callback, this, _1),
+            std::bind(&GripperCmd::accepted_callback, this, _1));
+
+        this->gripper_cmd_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>
+                                        ("/rasmt_hand_controller/command", 10);
+
+    }
+
+private:
+    rclcpp_action::Server<GripperCmdAction>::SharedPtr action_server_;
+    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_publisher_;
+
+    using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCmdAction>;
+
+    rclcpp_action::GoalResponse goal_callback(
+        const rclcpp_action::GoalUUID & uuid,
+        std::shared_ptr<const GripperCmdAction::Goal> goal)
+    {
+        RCLCPP_INFO(this->get_logger(), "Received goal request [%.3f]", goal->value);
+        (void)uuid;
+        if (false) {
+            return rclcpp_action::GoalResponse::REJECT;
+        }
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    }
+    
+    rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
+    {
+        RCLCPP_INFO(this->get_logger(), "Received cancel request");
+        (void)goal_handle;
+        return rclcpp_action::CancelResponse::ACCEPT;
+    }
+    
+    void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
+    {
+        using namespace std::placeholders;
+        std::thread(std::bind(&robossembler_servers::GripperCmd::execute, this, _1), goal_handle).detach();
+    }
+    
+    void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
+    {
+        RCLCPP_INFO(this->get_logger(), "Executing goal");
+        const auto goal = goal_handle->get_goal();
+        auto feedback = std::make_shared<GripperCmdAction::Feedback>();
+        auto result = std::make_shared<GripperCmdAction::Result>();
+    
+        if (goal_handle->is_canceling()) {
+            goal_handle->canceled(result);
+            RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
+            return;
+        }
+        std_msgs::msg::Float64MultiArray commands;
+        RCLCPP_INFO(this->get_logger(), "Initialize commands");
+        commands.data.push_back(goal->value);
+        commands.data.push_back(goal->value);
+        //commands.data[0] = goal->value;
+        //commands.data[1] = goal->value;
+        RCLCPP_INFO(this->get_logger(), "Publish commands");
+        this->gripper_cmd_publisher_->publish(commands);
+
+        goal_handle->succeed(result);
+        RCLCPP_INFO(this->get_logger(), "Successfully executed goal");
+    }
+
+};
+}  // namespace robossembler_servers
+
+RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_servers::GripperCmd)

robossembler_servers/src/gripper_cmd_srv.cpp

@@ -1,46 +0,0 @@
-#include <memory>
-#include <string>
-#include <vector>
-#include <cinttypes>
-
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_components/register_node_macro.hpp"
-#include "std_msgs/msg/float64_multi_array.hpp"
-#include "robossembler_interfaces/srv/gripper_command.hpp"
-
-using GripperCmdSrv = robossembler_interfaces::srv::GripperCommand;
-
-class GripperCmd : public rclcpp::Node
-{
-  public:
-    explicit GripperCmd(const rclcpp::NodeOptions & options)
-    : Node("gripper_cmd", options)
-    {
-        setvbuf(stdout, NULL, _IONBF, BUFSIZ);
-        auto handle_gripper_cmd = 
-        [this](const std::shared_ptr<rmw_request_id_t> request_header,
-        const std::shared_ptr<GripperCmdSrv::Request> request,
-        std::shared_ptr<GripperCmdSrv::Response> response) -> void
-        {
-            RCLCPP_INFO(this->get_logger(), "Service got request value [%f]", request->value);
-            (void)request_header;
-            RCLCPP_INFO(this->get_logger(), "request header");
-            //commands = request->value;
-            std_msgs::msg::Float64MultiArray msg;
-            RCLCPP_INFO(this->get_logger(), "init msg");
-            msg.data[0] = request->value;
-            RCLCPP_INFO(this->get_logger(), "write to msg");
-            RCLCPP_INFO(this->get_logger(), "Publishing message");
-            pub_->publish(msg);
-            response->success = "SUCCESS";
-
-        };
-        srv_ = create_service<GripperCmdSrv>("/gripper_cmd", handle_gripper_cmd);
-        pub_ = create_publisher<std_msgs::msg::Float64MultiArray>("/rasmt_hand_controller/commands", 10);
-    }
-  private:
-    rclcpp::Service<GripperCmdSrv>::SharedPtr srv_;
-    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr pub_;
-}; // class GripperCmd
-
-RCLCPP_COMPONENTS_REGISTER_NODE(GripperCmd)