add attacher

rasmt_support/world/robossembler.world

@@ -19,12 +19,9 @@
       </spot>
     </light>
     <plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">
-        <ros>
-            <!--namespace>/rasmt</namespace-->
-            <argument>model_states:=model_states_demo</argument>
-        </ros>
         <update_rate>1.0</update_rate>
     </plugin>
+    <plugin name="ros_link_attacher_plugin" filename="libgazebo_ros_link_attacher_plugin.so"/>
     <model name='ground_plane'>
       <static>1</static>
       <link name='link'>

robossembler_servers/CMakeLists.txt

@@ -28,6 +28,7 @@ find_package(tf2_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(robossembler_interfaces REQUIRED)
 find_package(rmw REQUIRED)
+find_package(gazebo_ros_link_attacher REQUIRED)
 
 set(deps
   rclcpp
@@ -40,6 +41,7 @@ set(deps
   tf2_ros
   rclcpp_components
   robossembler_interfaces
+  gazebo_ros_link_attacher
 )
 #
 # include_directories(include)
@@ -67,11 +69,14 @@ ament_target_dependencies(move_to_joint_states_action_server ${deps})
 add_executable(move_topose_action_server src/move_topose_action_server.cpp)
 ament_target_dependencies(move_topose_action_server ${deps})
 
+add_executable(move_cartesian_path_action_server src/move_cartesian_path_action_server.cpp)
+ament_target_dependencies(move_cartesian_path_action_server ${deps})
 
 install(
   TARGETS 
   move_topose_action_server
   move_to_joint_states_action_server
+  move_cartesian_path_action_server
   gripper_cmd_action_server
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

robossembler_servers/src/gripper_cmd.cpp

@@ -8,9 +8,11 @@
 #include "rclcpp_components/register_node_macro.hpp"
 #include "std_msgs/msg/float64_multi_array.hpp"
 #include "robossembler_interfaces/action/gripper_command.hpp"
+#include "gazebo_ros_link_attacher/srv/attach.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
 
 using GripperCmdAction = robossembler_interfaces::action::GripperCommand;
-
+double finger_state{0};
 namespace robossembler_servers
 {
 class GripperCmd : public rclcpp::Node
@@ -29,16 +31,24 @@ public:
             std::bind(&GripperCmd::accepted_callback, this, _1));
 
         this->gripper_cmd_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>
-                                        ("/rasmt_hand_controller/command", 10);
+                                        ("/rasmt_hand_controller/commands", 10);
+        this->attach_srv_ = this->create_client<gazebo_ros_link_attacher::srv::Attach>("attach");
+        this->detach_srv_ = this->create_client<gazebo_ros_link_attacher::srv::Attach>("detach");
 
+        this->joint_states_ = this->create_subscription<sensor_msgs::msg::JointState>
+                                ("/joint_states", 10, std::bind(&robossembler_servers::GripperCmd::get_fingers_state, this, _1));
     }
 
 private:
     rclcpp_action::Server<GripperCmdAction>::SharedPtr action_server_;
     rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_publisher_;
-
+    rclcpp::Client<gazebo_ros_link_attacher::srv::Attach>::SharedPtr attach_srv_;
+    rclcpp::Client<gazebo_ros_link_attacher::srv::Attach>::SharedPtr detach_srv_;
+    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_states_;
     using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCmdAction>;
 
+    //double finger_state{};
+
     rclcpp_action::GoalResponse goal_callback(
         const rclcpp_action::GoalUUID & uuid,
         std::shared_ptr<const GripperCmdAction::Goal> goal)
@@ -70,12 +80,22 @@ private:
         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;
         }
+        // Compute finger change for detach or attach
+        auto finger_state_change = goal->value - finger_state;
+        RCLCPP_ERROR(this->get_logger(), "Finger change [%.2f]", finger_state_change);
+        if(finger_state_change<0.0)
+        {
+            send_attach_goal("rasmt", "cube", "rasmt_Dock_Link", "link"); // TODO: read parameters from other sources
+        } else if (finger_state_change>0.0)
+        {
+            send_detach_goal("rasmt", "cube", "rasmt_Dock_Link", "link");
+        }
+        // Send commands to gripper
         std_msgs::msg::Float64MultiArray commands;
         RCLCPP_INFO(this->get_logger(), "Initialize commands");
         commands.data.push_back(goal->value);
@@ -89,6 +109,52 @@ private:
         RCLCPP_INFO(this->get_logger(), "Successfully executed goal");
     }
 
+    void send_attach_goal(const std::string model1_name, std::string model2_name,
+                            std::string model1_link_name, std::string model2_link_name)
+    {
+        auto request = std::make_shared<gazebo_ros_link_attacher::srv::Attach::Request>();
+        request->model_name_1 = model1_name;
+        request->model_name_2 = model2_name;
+        request->link_name_1 = model1_link_name;
+        request->link_name_2 = model2_link_name;
+
+        while (!attach_srv_->wait_for_service(std::chrono::seconds(1))) {
+            if (!rclcpp::ok()) {
+                RCLCPP_ERROR(this->get_logger(), "Interruped while waiting for the server.");
+                return;
+            }
+            RCLCPP_INFO(this->get_logger(), "Server not available, waiting again...");
+        }
+
+        auto result = attach_srv_->async_send_request(request);
+    }
+
+    void send_detach_goal(const std::string model1_name, std::string model2_name,
+                            std::string model1_link_name, std::string model2_link_name)
+    {
+        auto request = std::make_shared<gazebo_ros_link_attacher::srv::Attach::Request>();
+        request->model_name_1 = model1_name;
+        request->model_name_2 = model2_name;
+        request->link_name_1 = model1_link_name;
+        request->link_name_2 = model2_link_name;
+
+        while (!detach_srv_->wait_for_service(std::chrono::seconds(1))) {
+            if (!rclcpp::ok()) {
+                RCLCPP_ERROR(this->get_logger(), "Interruped while waiting for the server.");
+                return;
+            }
+            RCLCPP_INFO(this->get_logger(), "Server not available, waiting again...");
+        }
+
+        auto result = detach_srv_->async_send_request(request);
+    }
+
+    void get_fingers_state(const sensor_msgs::msg::JointState::SharedPtr msg) const
+    {
+        finger_state = msg->position.back();
+        //RCLCPP_INFO(this->get_logger(), "Current joint position [%.5f]", joint_state);
+    }
+
 };
 }  // namespace robossembler_servers