add gripper services

src/ros2aria/CMakeLists.txt

@@ -35,7 +35,8 @@ add_executable(ros2aria
   src/cmd_vel.cpp
   src/publish.cpp
   src/sonar.cpp
-  src/pose.cpp)
+  src/pose.cpp
+  src/gripper.cpp)
   
 ament_target_dependencies(ros2aria rclcpp)
 ament_target_dependencies(ros2aria std_srvs)

src/ros2aria/src/gripper.cpp

@@ -0,0 +1,34 @@
+#include "./ros2aria.hpp"
+
+void Ros2Aria::gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
+{
+    UNUSED(request);
+    UNUSED(response);
+    std::lock_guard<RAIIBot> lock(*robot.get());
+    auto g = robot->getGripper();
+    g->gripOpen();
+}
+void Ros2Aria::gripper_close_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
+{
+    UNUSED(request);
+    UNUSED(response);
+    std::lock_guard<RAIIBot> lock(*robot.get());
+    auto g = robot->getGripper();
+    g->gripClose();
+}
+void Ros2Aria::gripper_up_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
+{
+    UNUSED(request);
+    UNUSED(response);
+    std::lock_guard<RAIIBot> lock(*robot.get());
+    auto g = robot->getGripper();
+    g->liftUp();
+}
+void Ros2Aria::gripper_down_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
+{
+    UNUSED(request);
+    UNUSED(response);
+    std::lock_guard<RAIIBot> lock(*robot.get());
+    auto g = robot->getGripper();
+    g->liftDown();
+}

src/ros2aria/src/raiibot.cpp

@@ -15,6 +15,7 @@ public:
     RAIIBot(rclcpp::Node *node, std::string port)
     {
         this->robot = new ArRobot();
+        this->gripper = new ArGripper(this->robot);
         this->node = node;
         this->clock = rclcpp::Clock::make_shared();
 
@@ -76,6 +77,11 @@ public:
         return this->robot;
     }
 
+    ArGripper *getGripper()
+    {
+        return this->gripper;
+    }
+
     rclcpp::Clock::SharedPtr getClock()
     {
         return this->clock;
@@ -104,6 +110,7 @@ private:
     ArRobotConnector *robotConn = nullptr;
     ArArgumentBuilder *args = nullptr;
     ArArgumentParser *argparser = nullptr;
+    ArGripper *gripper = nullptr;
     rclcpp::Node *node = nullptr;
     rclcpp::TimerBase::SharedPtr watchdogTimer;
     rclcpp::Time lastWatchdogPoke;

src/ros2aria/src/ros2aria.cpp

@@ -12,13 +12,19 @@ Ros2Aria::Ros2Aria()
     RCLCPP_INFO(this->get_logger(), "starting subscribers and services");
     cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
         "cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1));
-    stop_service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&Ros2Aria::stop, this, _1, _2));
 
     sonar_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud>("sonar", 10);
     sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10);
 
     pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("pose", 1000);
 
+    // services
+    stop_service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&Ros2Aria::stop, this, _1, _2));
+    gripper_open_service_ = this->create_service<std_srvs::srv::Empty>("gripper_open", std::bind(&Ros2Aria::gripper_open_callback, this, _1, _2));
+    gripper_close_service_ = this->create_service<std_srvs::srv::Empty>("gripper_close", std::bind(&Ros2Aria::gripper_close_callback, this, _1, _2));
+    gripper_up_service_ = this->create_service<std_srvs::srv::Empty>("gripper_up", std::bind(&Ros2Aria::gripper_up_callback, this, _1, _2));
+    gripper_down_service_ = this->create_service<std_srvs::srv::Empty>("gripper_down", std::bind(&Ros2Aria::gripper_down_callback, this, _1, _2));
+
     // listen to sensors
     auto r = robot->getRobot();
     r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);

src/ros2aria/src/ros2aria.hpp

@@ -9,6 +9,8 @@
 
 #include "./raiibot.cpp"
 
+#define UNUSED(x) (void)(x)
+
 class Ros2Aria : public rclcpp::Node
 {
 public:
@@ -43,4 +45,16 @@ private:
     // services
     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;
     void stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const;
+
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr gripper_open_service_;
+    void gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const;
+
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr gripper_close_service_;
+    void gripper_close_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const;
+
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr gripper_up_service_;
+    void gripper_up_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const;
+
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr gripper_down_service_;
+    void gripper_down_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const;
 };