diff --git a/src/ros2aria/CMakeLists.txt b/src/ros2aria/CMakeLists.txt index f50c23b..0c9dbcd 100644 --- a/src/ros2aria/CMakeLists.txt +++ b/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) diff --git a/src/ros2aria/src/gripper.cpp b/src/ros2aria/src/gripper.cpp new file mode 100644 index 0000000..3a37561 --- /dev/null +++ b/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 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 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 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 lock(*robot.get()); + auto g = robot->getGripper(); + g->liftDown(); +} diff --git a/src/ros2aria/src/raiibot.cpp b/src/ros2aria/src/raiibot.cpp index 4e85b63..e5daf0f 100644 --- a/src/ros2aria/src/raiibot.cpp +++ b/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; diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp index ebc850d..8553328 100644 --- a/src/ros2aria/src/ros2aria.cpp +++ b/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( "cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1)); - stop_service_ = this->create_service("stop", std::bind(&Ros2Aria::stop, this, _1, _2)); sonar_pub_ = this->create_publisher("sonar", 10); sonar_pointcloud2_pub_ = this->create_publisher("sonar_pointcloud2", 10); pose_pub_ = this->create_publisher("pose", 1000); + // services + stop_service_ = this->create_service("stop", std::bind(&Ros2Aria::stop, this, _1, _2)); + gripper_open_service_ = this->create_service("gripper_open", std::bind(&Ros2Aria::gripper_open_callback, this, _1, _2)); + gripper_close_service_ = this->create_service("gripper_close", std::bind(&Ros2Aria::gripper_close_callback, this, _1, _2)); + gripper_up_service_ = this->create_service("gripper_up", std::bind(&Ros2Aria::gripper_up_callback, this, _1, _2)); + gripper_down_service_ = this->create_service("gripper_down", std::bind(&Ros2Aria::gripper_down_callback, this, _1, _2)); + // listen to sensors auto r = robot->getRobot(); r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb); diff --git a/src/ros2aria/src/ros2aria.hpp b/src/ros2aria/src/ros2aria.hpp index 49d045d..fb15bfa 100644 --- a/src/ros2aria/src/ros2aria.hpp +++ b/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::SharedPtr stop_service_; void stop(const std::shared_ptr request, std::shared_ptr response) const; + + rclcpp::Service::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::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::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::SharedPtr gripper_down_service_; + void gripper_down_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const; }; \ No newline at end of file