diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..5ad3bbab --- /dev/null +++ b/.clang-format @@ -0,0 +1,3 @@ +BasedOnStyle: Chromium +SortIncludes: false +ColumnLimit: 120 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 3b9dd1af..507a12c2 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -35,7 +35,7 @@ repos: args: [-w] - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.15.0 + rev: v0.15.1 hooks: - id: ruff-check args: [--fix, --exit-non-zero-on-fix] @@ -47,20 +47,20 @@ repos: - id: cmake-format - id: cmake-lint +- repo: https://github.com/pre-commit/mirrors-clang-format + rev: v21.1.8 + hooks: + - id: clang-format + types_or: [c++, proto] + - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.10.0 + rev: v0.10.1 hooks: - id: prettier-xacro - id: prettier-package-xml - id: ros-include-guard - id: sort-package-xml - # Protobuf & gRPC -- repo: https://github.com/bufbuild/buf - rev: v1.65.0 - hooks: - - id: buf-format - # TODO(issue#1) Re-enable warehouse integration # - repo: https://github.com/macmacal/pre-commit-sqlite-dump # rev: "1.0.0" diff --git a/aegis_grpc/include/aegis_grpc/robot_control_service.hpp b/aegis_grpc/include/aegis_grpc/robot_control_service.hpp index bd62e352..d371d610 100644 --- a/aegis_grpc/include/aegis_grpc/robot_control_service.hpp +++ b/aegis_grpc/include/aegis_grpc/robot_control_service.hpp @@ -18,110 +18,93 @@ #include #include - namespace aegis_grpc { -class RobotControlServiceImpl final - : public proto_aegis_grpc::v1::RobotControlService::Service { - public: - using GripperCommand = control_msgs::action::GripperCommand; - using GoalHandleGripper = rclcpp_action::ClientGoalHandle; - - enum class ServoMode { - None, - JointJog, - TCPTwist - }; - - explicit RobotControlServiceImpl(std::shared_ptr node); - - // TODO(issue#87) Create additional methods to enable and disable servo control. - grpc::Status ServoEnable( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::TriggerResponse* response) override; - - grpc::Status ServoDisable( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::TriggerResponse* response) override; - - grpc::Status ServoJoint( - grpc::ServerContext* context, - const proto_aegis_grpc::v1::JointJog* request, - google::protobuf::Empty* response) override; - - grpc::Status ServoTCP( - grpc::ServerContext* context, - const proto_aegis_grpc::v1::Twist* request, - google::protobuf::Empty* response) override; - - grpc::Status GotoPose( - grpc::ServerContext* context, - const proto_aegis_grpc::v1::Pose* request, - proto_aegis_grpc::v1::TriggerResponse* response) override; - - grpc::Status GotoJoints( - grpc::ServerContext* context, - const proto_aegis_grpc::v1::JointState* request, - proto_aegis_grpc::v1::TriggerResponse* response) override; - - grpc::Status GripperSetPosition( - grpc::ServerContext* context, - const proto_aegis_grpc::v1::GripperSetPositionRequest* request, - proto_aegis_grpc::v1::TriggerResponse* response) override; - - grpc::Status GripperClose( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::TriggerResponse* response) override; - - grpc::Status GripperOpen( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::TriggerResponse* response) override; - - private: - bool SwitchControllers( - const std::vector& activate, - const std::vector& deactivate); - bool CallServoStartService(); - bool CallServoStopService(); - - void ServoPublishLoop(); - void GripperSendGoal(double position, double max_effort); - - rclcpp::Logger get_logger() const; - - template - void DeclareROSParameter(const std::string& name, const T& default_val, const std::string& description); - - std::shared_ptr node_; - std::unique_ptr move_group_; - - rclcpp::Client::SharedPtr switch_controller_client_; - rclcpp::Client::SharedPtr start_servo_client_; - rclcpp::Client::SharedPtr stop_servo_client_; - rclcpp::Publisher::SharedPtr servo_joint_pub_; - rclcpp::Publisher::SharedPtr servo_tcp_pub_; - rclcpp_action::Client::SharedPtr gripper_client_; - rclcpp::TimerBase::SharedPtr servo_pub_timer_; - - std::mutex servo_mutex_; - ServoMode servo_mode_; - std::string servo_tcp_link_; - control_msgs::msg::JointJog servo_joint_msg_; - geometry_msgs::msg::Twist servo_tcp_msg_; - int servo_frequency_ratio_; - int servo_msgs_left_; - - std::mutex gripper_mutex_; - std::atomic gripper_in_use_; - std::chrono::duration action_timeout_; - std::string gripper_cmd_msg_; - bool gripper_cmd_success_; - std::atomic gripper_cmd_done_; +class RobotControlServiceImpl final : public proto_aegis_grpc::v1::RobotControlService::Service { + public: + using GripperCommand = control_msgs::action::GripperCommand; + using GoalHandleGripper = rclcpp_action::ClientGoalHandle; + + enum class ServoMode { None, JointJog, TCPTwist }; + + explicit RobotControlServiceImpl(std::shared_ptr node); + + // TODO(issue#87) Create additional methods to enable and disable servo control. + grpc::Status ServoEnable(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::TriggerResponse* response) override; + + grpc::Status ServoDisable(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::TriggerResponse* response) override; + + grpc::Status ServoJoint(grpc::ServerContext* context, + const proto_aegis_grpc::v1::JointJog* request, + google::protobuf::Empty* response) override; + + grpc::Status ServoTCP(grpc::ServerContext* context, + const proto_aegis_grpc::v1::Twist* request, + google::protobuf::Empty* response) override; + + grpc::Status GotoPose(grpc::ServerContext* context, + const proto_aegis_grpc::v1::Pose* request, + proto_aegis_grpc::v1::TriggerResponse* response) override; + + grpc::Status GotoJoints(grpc::ServerContext* context, + const proto_aegis_grpc::v1::JointState* request, + proto_aegis_grpc::v1::TriggerResponse* response) override; + + grpc::Status GripperSetPosition(grpc::ServerContext* context, + const proto_aegis_grpc::v1::GripperSetPositionRequest* request, + proto_aegis_grpc::v1::TriggerResponse* response) override; + + grpc::Status GripperClose(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::TriggerResponse* response) override; + + grpc::Status GripperOpen(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::TriggerResponse* response) override; + + private: + bool SwitchControllers(const std::vector& activate, const std::vector& deactivate); + bool CallServoStartService(); + bool CallServoStopService(); + + void ServoPublishLoop(); + void GripperSendGoal(double position, double max_effort); + + rclcpp::Logger get_logger() const; + + template + void DeclareROSParameter(const std::string& name, const T& default_val, const std::string& description); + + std::shared_ptr node_; + std::unique_ptr move_group_; + + rclcpp::Client::SharedPtr switch_controller_client_; + rclcpp::Client::SharedPtr start_servo_client_; + rclcpp::Client::SharedPtr stop_servo_client_; + rclcpp::Publisher::SharedPtr servo_joint_pub_; + rclcpp::Publisher::SharedPtr servo_tcp_pub_; + rclcpp_action::Client::SharedPtr gripper_client_; + rclcpp::TimerBase::SharedPtr servo_pub_timer_; + + std::mutex servo_mutex_; + ServoMode servo_mode_; + std::string servo_tcp_link_; + control_msgs::msg::JointJog servo_joint_msg_; + geometry_msgs::msg::Twist servo_tcp_msg_; + int servo_frequency_ratio_; + int servo_msgs_left_; + + std::mutex gripper_mutex_; + std::atomic gripper_in_use_; + std::chrono::duration action_timeout_; + std::string gripper_cmd_msg_; + bool gripper_cmd_success_; + std::atomic gripper_cmd_done_; }; -} // namespace aegis_grpc +} // namespace aegis_grpc #endif // AEGIS_GRPC__ROBOT_CONTROL_SERVICE_HPP_ diff --git a/aegis_grpc/include/aegis_grpc/robot_read_service.hpp b/aegis_grpc/include/aegis_grpc/robot_read_service.hpp index 7bee92e4..54c7004c 100644 --- a/aegis_grpc/include/aegis_grpc/robot_read_service.hpp +++ b/aegis_grpc/include/aegis_grpc/robot_read_service.hpp @@ -18,127 +18,99 @@ namespace aegis_grpc { -class RobotReadServiceImpl final - : public proto_aegis_grpc::v1::RobotReadService::Service { - public: - explicit RobotReadServiceImpl(std::shared_ptr node); - - grpc::Status GetTCPPose( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::Pose* response - ) override; - - grpc::Status GetWrench( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::Wrench* response - ) override; - - grpc::Status GetJointStates( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::JointState* response - ) override; - - grpc::Status GetCameraSceneImage( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::Image* response - ) override; - - grpc::Status GetCameraRightImage( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::Image* response - ) override; - - grpc::Status GetCameraLeftImage( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::Image* response - ) override; - - grpc::Status GetRobotState( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::RobotState* response - ) override; - - grpc::Status GetRobotVision( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::RobotVision* response - ) override; - - grpc::Status GetAll( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::RobotObservation* response - ) override; - - // TODO(issue#84) implement getters for images from cameras (RGB & RGBD) - // TODO(issue#85) develop synchronization guard to monitor the freshness of the data - - private: - template - void DeclareROSParameter(const std::string& name, const T& default_val, const std::string& description); - - void PoseSubCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg); - void WrenchSubCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); - void JointStateSubCb(const sensor_msgs::msg::JointState::SharedPtr msg); - void ImageSceneSubCb(const sensor_msgs::msg::Image::SharedPtr msg); - void ImageRightSubCb(const sensor_msgs::msg::Image::SharedPtr msg); - void ImageLeftSubCb(const sensor_msgs::msg::Image::SharedPtr msg); - - static void FillProtoPose( - const geometry_msgs::msg::Pose& ros, - proto_aegis_grpc::v1::Pose* out); - - static void FillProtoWrench( - const geometry_msgs::msg::Wrench& ros, - proto_aegis_grpc::v1::Wrench* out); - - static void FillProtoJointState( - const sensor_msgs::msg::JointState& ros, - proto_aegis_grpc::v1::JointState* out); - - void FillProtoImage( - const sensor_msgs::msg::Image& ros, - proto_aegis_grpc::v1::Image* out, - cv::Mat& resize_buffer); - - std::shared_ptr node_; - rclcpp::Subscription::SharedPtr pose_sub_; - rclcpp::Subscription::SharedPtr wrench_sub_; - rclcpp::Subscription::SharedPtr joint_state_sub_; - rclcpp::Subscription::SharedPtr image_scene_sub_; - rclcpp::Subscription::SharedPtr image_right_sub_; - rclcpp::Subscription::SharedPtr image_left_sub_; - - geometry_msgs::msg::Pose pose_data_; - geometry_msgs::msg::Wrench wrench_data_; - sensor_msgs::msg::JointState joint_state_data_; - sensor_msgs::msg::Image image_scene_data_; - sensor_msgs::msg::Image image_right_data_; - sensor_msgs::msg::Image image_left_data_; - - std::mutex pose_mutex_; - std::mutex wrench_mutex_; - std::mutex joint_state_mutex_; - std::mutex image_scene_mutex_; - std::mutex image_right_mutex_; - std::mutex image_left_mutex_; - - uint32_t target_img_width_; - uint32_t target_img_height_; - - cv::Mat scene_resized_; - cv::Mat right_resized_; - cv::Mat left_resized_; - - bool enable_image_resize_; +class RobotReadServiceImpl final : public proto_aegis_grpc::v1::RobotReadService::Service { + public: + explicit RobotReadServiceImpl(std::shared_ptr node); + + grpc::Status GetTCPPose(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::Pose* response) override; + + grpc::Status GetWrench(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::Wrench* response) override; + + grpc::Status GetJointStates(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::JointState* response) override; + + grpc::Status GetCameraSceneImage(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::Image* response) override; + + grpc::Status GetCameraRightImage(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::Image* response) override; + + grpc::Status GetCameraLeftImage(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::Image* response) override; + + grpc::Status GetRobotState(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::RobotState* response) override; + + grpc::Status GetRobotVision(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::RobotVision* response) override; + + grpc::Status GetAll(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::RobotObservation* response) override; + + // TODO(issue#84) implement getters for images from cameras (RGB & RGBD) + // TODO(issue#85) develop synchronization guard to monitor the freshness of the data + + private: + template + void DeclareROSParameter(const std::string& name, const T& default_val, const std::string& description); + + void PoseSubCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void WrenchSubCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg); + void JointStateSubCb(const sensor_msgs::msg::JointState::SharedPtr msg); + void ImageSceneSubCb(const sensor_msgs::msg::Image::SharedPtr msg); + void ImageRightSubCb(const sensor_msgs::msg::Image::SharedPtr msg); + void ImageLeftSubCb(const sensor_msgs::msg::Image::SharedPtr msg); + + static void FillProtoPose(const geometry_msgs::msg::Pose& ros, proto_aegis_grpc::v1::Pose* out); + + static void FillProtoWrench(const geometry_msgs::msg::Wrench& ros, proto_aegis_grpc::v1::Wrench* out); + + static void FillProtoJointState(const sensor_msgs::msg::JointState& ros, proto_aegis_grpc::v1::JointState* out); + + void FillProtoImage(const sensor_msgs::msg::Image& ros, proto_aegis_grpc::v1::Image* out, cv::Mat& resize_buffer); + + std::shared_ptr node_; + rclcpp::Subscription::SharedPtr pose_sub_; + rclcpp::Subscription::SharedPtr wrench_sub_; + rclcpp::Subscription::SharedPtr joint_state_sub_; + rclcpp::Subscription::SharedPtr image_scene_sub_; + rclcpp::Subscription::SharedPtr image_right_sub_; + rclcpp::Subscription::SharedPtr image_left_sub_; + + geometry_msgs::msg::Pose pose_data_; + geometry_msgs::msg::Wrench wrench_data_; + sensor_msgs::msg::JointState joint_state_data_; + sensor_msgs::msg::Image image_scene_data_; + sensor_msgs::msg::Image image_right_data_; + sensor_msgs::msg::Image image_left_data_; + + std::mutex pose_mutex_; + std::mutex wrench_mutex_; + std::mutex joint_state_mutex_; + std::mutex image_scene_mutex_; + std::mutex image_right_mutex_; + std::mutex image_left_mutex_; + + uint32_t target_img_width_; + uint32_t target_img_height_; + + cv::Mat scene_resized_; + cv::Mat right_resized_; + cv::Mat left_resized_; + + bool enable_image_resize_; }; -} // namespace aegis_grpc +} // namespace aegis_grpc #endif // AEGIS_GRPC__ROBOT_READ_SERVICE_HPP_ diff --git a/aegis_grpc/proto_aegis_grpc/v1/robot_srvs.proto b/aegis_grpc/proto_aegis_grpc/v1/robot_srvs.proto index 9df15b6d..246009d2 100644 --- a/aegis_grpc/proto_aegis_grpc/v1/robot_srvs.proto +++ b/aegis_grpc/proto_aegis_grpc/v1/robot_srvs.proto @@ -56,6 +56,6 @@ message TriggerResponse { // https://github.com/ros-controls/control_msgs/blob/humble/control_msgs/action/GripperCommand.action message GripperSetPositionRequest { - double position = 1; // The goal gripper gap size (in meters) - double effort = 2; // The goal effort exerted (in Newtons) + double position = 1; // The goal gripper gap size (in meters) + double effort = 2; // The goal effort exerted (in Newtons) } diff --git a/aegis_grpc/src/grpc_server_node.cpp b/aegis_grpc/src/grpc_server_node.cpp index 5d36698e..cf28e50d 100644 --- a/aegis_grpc/src/grpc_server_node.cpp +++ b/aegis_grpc/src/grpc_server_node.cpp @@ -10,16 +10,13 @@ #include "aegis_grpc/robot_control_service.hpp" #include "aegis_grpc/robot_read_service.hpp" -std::unique_ptr -BuildServer(const std::string &address, - std::vector &services) { - +std::unique_ptr BuildServer(const std::string& address, std::vector& services) { grpc::ServerBuilder builder; grpc::reflection::InitProtoReflectionServerBuilderPlugin(); grpc::EnableDefaultHealthCheckService(true); builder.AddListeningPort(address, grpc::InsecureServerCredentials()); - for (auto *srv : services) + for (auto* srv : services) builder.RegisterService(srv); return builder.BuildAndStart(); @@ -33,7 +30,7 @@ std::string GetServerAdress(std::shared_ptr node) { return "0.0.0.0:" + port; } -int main(int argc, char *argv[]) { +int main(int argc, char* argv[]) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("grpc_server"); auto logger = node->get_logger(); @@ -47,7 +44,7 @@ int main(int argc, char *argv[]) { aegis_grpc::RobotControlServiceImpl control_service(node); RCLCPP_INFO(logger, "Setting up gRPC server with services"); - std::vector services = {&read_service, &control_service}; + std::vector services = {&read_service, &control_service}; auto server = BuildServer(address, services); std::thread grpc_thread([&server]() { server->Wait(); }); RCLCPP_INFO(logger, "gRPC server listening on %s", address.c_str()); diff --git a/aegis_grpc/src/robot_control_service.cpp b/aegis_grpc/src/robot_control_service.cpp index 3a917e16..f56089b8 100644 --- a/aegis_grpc/src/robot_control_service.cpp +++ b/aegis_grpc/src/robot_control_service.cpp @@ -5,11 +5,15 @@ using namespace std::chrono_literals; namespace aegis_grpc { -RobotControlServiceImpl::RobotControlServiceImpl( - std::shared_ptr node) - : node_(node), servo_mode_(ServoMode::None), servo_frequency_ratio_(0), servo_msgs_left_(0), gripper_in_use_(false), - action_timeout_(0.0), gripper_cmd_success_(false), gripper_cmd_done_(false) { - +RobotControlServiceImpl::RobotControlServiceImpl(std::shared_ptr node) + : node_(node), + servo_mode_(ServoMode::None), + servo_frequency_ratio_(0), + servo_msgs_left_(0), + gripper_in_use_(false), + action_timeout_(0.0), + gripper_cmd_success_(false), + gripper_cmd_done_(false) { // Initialization parameters DeclareROSParameter("servo_link", std::string("base_link"), "[str] Init; Name of the base link for the TCP servoing."); @@ -35,21 +39,22 @@ RobotControlServiceImpl::RobotControlServiceImpl( servo_frequency_ratio_ = std::round(servo_out_hz / servo_in_hz); RCLCPP_INFO(get_logger(), "> Frequency ratio for servo re-publishig is %i", servo_frequency_ratio_); - switch_controller_client_ = node_->create_client("/controller_manager/switch_controller"); + switch_controller_client_ = + node_->create_client("/controller_manager/switch_controller"); start_servo_client_ = node_->create_client("/servo_node/start_servo"); stop_servo_client_ = node_->create_client("/servo_node/stop_servo"); - servo_joint_pub_ = node_->create_publisher( - node_->get_parameter("topic_servo_joint").as_string(), 10); + servo_joint_pub_ = + node_->create_publisher(node_->get_parameter("topic_servo_joint").as_string(), 10); servo_tcp_pub_ = node_->create_publisher( node_->get_parameter("topic_servo_tcp").as_string(), 10); - gripper_client_ = rclcpp_action::create_client( - node_, node_->get_parameter("action_gripper").as_string()); + gripper_client_ = + rclcpp_action::create_client(node_, node_->get_parameter("action_gripper").as_string()); double hz = node_->get_parameter("servo_out_rate_hz").as_double(); auto servo_pub_period = std::chrono::duration(1.0 / hz); - servo_pub_timer_ = node_->create_wall_timer( - servo_pub_period, std::bind(&RobotControlServiceImpl::ServoPublishLoop, this)); + servo_pub_timer_ = + node_->create_wall_timer(servo_pub_period, std::bind(&RobotControlServiceImpl::ServoPublishLoop, this)); auto action_timeout = node_->get_parameter("action_timeout_s").as_double(); action_timeout_ = std::chrono::duration(action_timeout); @@ -63,24 +68,20 @@ rclcpp::Logger RobotControlServiceImpl::get_logger() const { } template -void RobotControlServiceImpl::DeclareROSParameter( - const std::string &name, - const T& default_val, - const std::string &description) { +void RobotControlServiceImpl::DeclareROSParameter(const std::string& name, + const T& default_val, + const std::string& description) { auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; param_desc.description = description; node_->declare_parameter(name, default_val, param_desc); const auto p = node_->get_parameter(name); - RCLCPP_INFO(get_logger(), "> %s := %s", - name.c_str(), - p.value_to_string().c_str()); + RCLCPP_INFO(get_logger(), "> %s := %s", name.c_str(), p.value_to_string().c_str()); } -bool RobotControlServiceImpl::SwitchControllers( - const std::vector& activate, - const std::vector& deactivate) { +bool RobotControlServiceImpl::SwitchControllers(const std::vector& activate, + const std::vector& deactivate) { auto req = std::make_shared(); req->activate_controllers = activate; @@ -137,16 +138,12 @@ bool RobotControlServiceImpl::CallServoStopService() { return result->success; } -grpc::Status RobotControlServiceImpl::ServoEnable( - grpc::ServerContext*, - const google::protobuf::Empty*, - proto_aegis_grpc::v1::TriggerResponse* response) -{ +grpc::Status RobotControlServiceImpl::ServoEnable(grpc::ServerContext*, + const google::protobuf::Empty*, + proto_aegis_grpc::v1::TriggerResponse* response) { response->set_success(false); - if (!SwitchControllers( - {"forward_position_controller"}, - {"scaled_joint_trajectory_controller"})) { + if (!SwitchControllers({"forward_position_controller"}, {"scaled_joint_trajectory_controller"})) { std::string err = "Controller switch failed."; RCLCPP_WARN(get_logger(), "[RobotControlService][ServoEnable] %s", err.c_str()); response->set_msg(err); @@ -171,11 +168,9 @@ grpc::Status RobotControlServiceImpl::ServoEnable( return grpc::Status::OK; } -grpc::Status RobotControlServiceImpl::ServoDisable( - grpc::ServerContext*, - const google::protobuf::Empty*, - proto_aegis_grpc::v1::TriggerResponse* response) -{ +grpc::Status RobotControlServiceImpl::ServoDisable(grpc::ServerContext*, + const google::protobuf::Empty*, + proto_aegis_grpc::v1::TriggerResponse* response) { response->set_success(false); if (!CallServoStopService()) { @@ -185,10 +180,7 @@ grpc::Status RobotControlServiceImpl::ServoDisable( return grpc::Status(grpc::StatusCode::INTERNAL, err); } - if (!SwitchControllers( - {"scaled_joint_trajectory_controller"}, - {"forward_position_controller"})) { - + if (!SwitchControllers({"scaled_joint_trajectory_controller"}, {"forward_position_controller"})) { std::string err = "Controller switch failed."; RCLCPP_WARN(get_logger(), "[RobotControlService][ServoDisable] %s", err.c_str()); response->set_msg(err); @@ -214,13 +206,13 @@ void RobotControlServiceImpl::ServoPublishLoop() { { std::lock_guard lock(servo_mutex_); - if(servo_msgs_left_ == 0) { + if (servo_msgs_left_ == 0) { servo_mode_ = ServoMode::None; return; } mode = servo_mode_; - switch(mode) { + switch (mode) { case ServoMode::JointJog: jog_msg = servo_joint_msg_; break; @@ -234,7 +226,7 @@ void RobotControlServiceImpl::ServoPublishLoop() { servo_msgs_left_--; } - switch(mode) { + switch (mode) { case ServoMode::JointJog: jog_msg.header.stamp = node_->now(); servo_joint_pub_->publish(jog_msg); @@ -251,26 +243,21 @@ void RobotControlServiceImpl::ServoPublishLoop() { } } -grpc::Status RobotControlServiceImpl::ServoJoint( - grpc::ServerContext *context, const proto_aegis_grpc::v1::JointJog *request, - google::protobuf::Empty *response) { - +grpc::Status RobotControlServiceImpl::ServoJoint(grpc::ServerContext* context, + const proto_aegis_grpc::v1::JointJog* request, + google::protobuf::Empty* response) { (void)context; (void)response; auto N = request->joint_names_size(); if (N != request->displacements_size()) { - std::string err = - "The number of joints mismatches the number of displacmenets values!"; - RCLCPP_WARN(get_logger(), "[RobotControlService][ServoJoint] %s", - err.c_str()); + std::string err = "The number of joints mismatches the number of displacmenets values!"; + RCLCPP_WARN(get_logger(), "[RobotControlService][ServoJoint] %s", err.c_str()); return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, err); } if (N != request->velocities_size()) { - std::string err = - "The number of joints mismatches the number of velocities values!"; - RCLCPP_WARN(get_logger(), "[RobotControlService][ServoJoint] %s", - err.c_str()); + std::string err = "The number of joints mismatches the number of velocities values!"; + RCLCPP_WARN(get_logger(), "[RobotControlService][ServoJoint] %s", err.c_str()); return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, err); } @@ -296,16 +283,14 @@ grpc::Status RobotControlServiceImpl::ServoJoint( return grpc::Status::OK; } -grpc::Status -RobotControlServiceImpl::ServoTCP(grpc::ServerContext *context, - const proto_aegis_grpc::v1::Twist *request, - google::protobuf::Empty *response) { - +grpc::Status RobotControlServiceImpl::ServoTCP(grpc::ServerContext* context, + const proto_aegis_grpc::v1::Twist* request, + google::protobuf::Empty* response) { (void)context; (void)response; - const auto &lin = request->linear(); - const auto &ang = request->angular(); + const auto& lin = request->linear(); + const auto& ang = request->angular(); auto ros_msg = geometry_msgs::msg::Twist(); ros_msg.linear.x = lin.x(); @@ -324,16 +309,14 @@ RobotControlServiceImpl::ServoTCP(grpc::ServerContext *context, return grpc::Status::OK; } -grpc::Status RobotControlServiceImpl::GripperSetPosition( - grpc::ServerContext *context, - const proto_aegis_grpc::v1::GripperSetPositionRequest *request, - proto_aegis_grpc::v1::TriggerResponse *response) { - +grpc::Status RobotControlServiceImpl::GripperSetPosition(grpc::ServerContext* context, + const proto_aegis_grpc::v1::GripperSetPositionRequest* request, + proto_aegis_grpc::v1::TriggerResponse* response) { (void)context; { std::lock_guard lock(gripper_mutex_); - if(gripper_in_use_.load(std::memory_order_relaxed)){ + if (gripper_in_use_.load(std::memory_order_relaxed)) { const std::string msg = "Gripper is already in use."; response->set_success(false); response->set_msg(msg); @@ -342,7 +325,6 @@ grpc::Status RobotControlServiceImpl::GripperSetPosition( gripper_in_use_.store(true, std::memory_order_relaxed); } - const double position = request->position(); const double effort = request->effort(); GripperSendGoal(position, effort); @@ -354,15 +336,15 @@ grpc::Status RobotControlServiceImpl::GripperSetPosition( return grpc::Status::OK; } -grpc::Status RobotControlServiceImpl::GripperClose( - grpc::ServerContext *context, const google::protobuf::Empty *request, - proto_aegis_grpc::v1::TriggerResponse *response) { +grpc::Status RobotControlServiceImpl::GripperClose(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::TriggerResponse* response) { (void)context; (void)request; { std::lock_guard lock(gripper_mutex_); - if(gripper_in_use_.load(std::memory_order_relaxed)){ + if (gripper_in_use_.load(std::memory_order_relaxed)) { const std::string msg = "Gripper is already in use."; response->set_success(false); response->set_msg(msg); @@ -380,16 +362,15 @@ grpc::Status RobotControlServiceImpl::GripperClose( return grpc::Status::OK; } -grpc::Status RobotControlServiceImpl::GripperOpen( - grpc::ServerContext *context, const google::protobuf::Empty *request, - proto_aegis_grpc::v1::TriggerResponse *response) { - +grpc::Status RobotControlServiceImpl::GripperOpen(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::TriggerResponse* response) { (void)context; (void)request; { std::lock_guard lock(gripper_mutex_); - if(gripper_in_use_.load(std::memory_order_relaxed)){ + if (gripper_in_use_.load(std::memory_order_relaxed)) { const std::string msg = "Gripper is already in use."; response->set_success(false); response->set_msg(msg); @@ -407,10 +388,9 @@ grpc::Status RobotControlServiceImpl::GripperOpen( return grpc::Status::OK; } -grpc::Status RobotControlServiceImpl::GotoPose( - grpc::ServerContext* context, - const proto_aegis_grpc::v1::Pose *request, - proto_aegis_grpc::v1::TriggerResponse *response) { +grpc::Status RobotControlServiceImpl::GotoPose(grpc::ServerContext* context, + const proto_aegis_grpc::v1::Pose* request, + proto_aegis_grpc::v1::TriggerResponse* response) { (void)context; response->set_success(false); @@ -443,11 +423,11 @@ grpc::Status RobotControlServiceImpl::GotoPose( } auto exec = move_group_->execute(plan); - if(exec != moveit::core::MoveItErrorCode::SUCCESS) { - std::string err = "Execution failed."; - RCLCPP_WARN(get_logger(), "[RobotControlService][GoToPose] %s", err.c_str()); - response->set_msg(err); - return grpc::Status(grpc::StatusCode::INTERNAL, err); + if (exec != moveit::core::MoveItErrorCode::SUCCESS) { + std::string err = "Execution failed."; + RCLCPP_WARN(get_logger(), "[RobotControlService][GoToPose] %s", err.c_str()); + response->set_msg(err); + return grpc::Status(grpc::StatusCode::INTERNAL, err); } response->set_success(true); @@ -455,10 +435,9 @@ grpc::Status RobotControlServiceImpl::GotoPose( return grpc::Status::OK; } -grpc::Status RobotControlServiceImpl::GotoJoints( - grpc::ServerContext* context, - const proto_aegis_grpc::v1::JointState *request, - proto_aegis_grpc::v1::TriggerResponse *response) { +grpc::Status RobotControlServiceImpl::GotoJoints(grpc::ServerContext* context, + const proto_aegis_grpc::v1::JointState* request, + proto_aegis_grpc::v1::TriggerResponse* response) { (void)context; response->set_success(false); @@ -472,7 +451,7 @@ grpc::Status RobotControlServiceImpl::GotoJoints( std::map target; for (int i = 0; i < request->name_size(); ++i) { - target[request->name(i)] = request->position(i); + target[request->name(i)] = request->position(i); } move_group_->setJointValueTarget(target); @@ -487,11 +466,11 @@ grpc::Status RobotControlServiceImpl::GotoJoints( } auto exec = move_group_->execute(plan); - if(exec != moveit::core::MoveItErrorCode::SUCCESS) { - std::string err = "Execution failed."; - RCLCPP_WARN(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str()); - response->set_msg(err); - return grpc::Status(grpc::StatusCode::INTERNAL, err); + if (exec != moveit::core::MoveItErrorCode::SUCCESS) { + std::string err = "Execution failed."; + RCLCPP_WARN(get_logger(), "[RobotControlService][GotoJoints] %s", err.c_str()); + response->set_msg(err); + return grpc::Status(grpc::StatusCode::INTERNAL, err); } response->set_success(true); @@ -499,8 +478,7 @@ grpc::Status RobotControlServiceImpl::GotoJoints( return grpc::Status::OK; } -void RobotControlServiceImpl::GripperSendGoal(double position, - double max_effort) { +void RobotControlServiceImpl::GripperSendGoal(double position, double max_effort) { if (!gripper_client_->wait_for_action_server(action_timeout_)) { gripper_cmd_success_ = false; gripper_cmd_msg_ = "Gripper Controller action server is not available. Skipping goal."; @@ -514,36 +492,34 @@ void RobotControlServiceImpl::GripperSendGoal(double position, rclcpp_action::Client::SendGoalOptions options; - options.result_callback = - [this](const GoalHandleGripper::WrappedResult &result) { - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - gripper_cmd_success_ = true; - gripper_cmd_msg_ = ""; - gripper_cmd_done_.store(true, std::memory_order_relaxed); - return; - case rclcpp_action::ResultCode::ABORTED: - gripper_cmd_msg_ = "ABORTED"; - break; - case rclcpp_action::ResultCode::CANCELED: - gripper_cmd_msg_ = "CANCELED"; - break; - default: - gripper_cmd_msg_ = "UNKNOWN"; - break; - } - gripper_cmd_success_ = false; + options.result_callback = [this](const GoalHandleGripper::WrappedResult& result) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + gripper_cmd_success_ = true; + gripper_cmd_msg_ = ""; gripper_cmd_done_.store(true, std::memory_order_relaxed); - RCLCPP_ERROR(get_logger(), "Gripper goal result: %s", - gripper_cmd_msg_.c_str()); - }; + return; + case rclcpp_action::ResultCode::ABORTED: + gripper_cmd_msg_ = "ABORTED"; + break; + case rclcpp_action::ResultCode::CANCELED: + gripper_cmd_msg_ = "CANCELED"; + break; + default: + gripper_cmd_msg_ = "UNKNOWN"; + break; + } + gripper_cmd_success_ = false; + gripper_cmd_done_.store(true, std::memory_order_relaxed); + RCLCPP_ERROR(get_logger(), "Gripper goal result: %s", gripper_cmd_msg_.c_str()); + }; gripper_cmd_done_.store(false, std::memory_order_relaxed); gripper_client_->async_send_goal(goal, options); - while(!gripper_cmd_done_.load(std::memory_order_relaxed)){ + while (!gripper_cmd_done_.load(std::memory_order_relaxed)) { std::this_thread::sleep_for(1ms); } } -} // namespace aegis_grpc +} // namespace aegis_grpc diff --git a/aegis_grpc/src/robot_read_service.cpp b/aegis_grpc/src/robot_read_service.cpp index 533b39f5..2e037a97 100644 --- a/aegis_grpc/src/robot_read_service.cpp +++ b/aegis_grpc/src/robot_read_service.cpp @@ -5,334 +5,294 @@ namespace aegis_grpc { RobotReadServiceImpl::RobotReadServiceImpl(std::shared_ptr node) : node_(node), pose_data_(), wrench_data_(), joint_state_data_() { - // Initialization parameters DeclareROSParameter("topic_pose", std::string("/tcp_pose"), "[str] Init; Sub: topic with the TCP pose data."); DeclareROSParameter("topic_wrench", std::string("/wrench"), "[str] Init; Sub: topic with the F/T data."); DeclareROSParameter("topic_joints", std::string("/joint_states"), "[str] Init; Sub: topic with the joint states."); - DeclareROSParameter("topic_camera_scene", std::string("/cam_scene/rgb/image_rect"), "[str] Init; Sub: Camera scene image topic."); - DeclareROSParameter("topic_camera_right", std::string("/cam_tool_right/image_color"), "[str] Init; Sub: Camera scene image topic."); - DeclareROSParameter("topic_camera_left", std::string("/cam_tool_left/image_color"), "[str] Init; Sub:Camera scene image topic."); + DeclareROSParameter("topic_camera_scene", std::string("/cam_scene/rgb/image_rect"), + "[str] Init; Sub: Camera scene image topic."); + DeclareROSParameter("topic_camera_right", std::string("/cam_tool_right/image_color"), + "[str] Init; Sub: Camera scene image topic."); + DeclareROSParameter("topic_camera_left", std::string("/cam_tool_left/image_color"), + "[str] Init; Sub:Camera scene image topic."); DeclareROSParameter("target_image_width", 64, "[int] Init; Target output image width in pixels."); DeclareROSParameter("target_image_height", 64, "[int] Init; Target output image height in pixels."); DeclareROSParameter("enable_image_resize", true, "[bool] Init; Enable resizing images before sending over gRPC."); pose_sub_ = node_->create_subscription( node_->get_parameter("topic_pose").as_string(), 10, - std::bind(&RobotReadServiceImpl::PoseSubCb, this, - std::placeholders::_1)); + std::bind(&RobotReadServiceImpl::PoseSubCb, this, std::placeholders::_1)); wrench_sub_ = node_->create_subscription( node_->get_parameter("topic_wrench").as_string(), 10, - std::bind(&RobotReadServiceImpl::WrenchSubCb, this, - std::placeholders::_1)); + std::bind(&RobotReadServiceImpl::WrenchSubCb, this, std::placeholders::_1)); joint_state_sub_ = node_->create_subscription( node_->get_parameter("topic_joints").as_string(), 10, - std::bind(&RobotReadServiceImpl::JointStateSubCb, this, - std::placeholders::_1)); + std::bind(&RobotReadServiceImpl::JointStateSubCb, this, std::placeholders::_1)); image_scene_sub_ = node_->create_subscription( node_->get_parameter("topic_camera_scene").as_string(), 10, - std::bind(&RobotReadServiceImpl::ImageSceneSubCb, this, - std::placeholders::_1)); + std::bind(&RobotReadServiceImpl::ImageSceneSubCb, this, std::placeholders::_1)); image_right_sub_ = node_->create_subscription( node_->get_parameter("topic_camera_right").as_string(), 10, - std::bind(&RobotReadServiceImpl::ImageRightSubCb, this, - std::placeholders::_1)); + std::bind(&RobotReadServiceImpl::ImageRightSubCb, this, std::placeholders::_1)); image_left_sub_ = node_->create_subscription( node_->get_parameter("topic_camera_left").as_string(), 10, - std::bind(&RobotReadServiceImpl::ImageLeftSubCb, this, - std::placeholders::_1)); + std::bind(&RobotReadServiceImpl::ImageLeftSubCb, this, std::placeholders::_1)); target_img_width_ = node_->get_parameter("target_image_width").as_int(); target_img_height_ = node_->get_parameter("target_image_height").as_int(); enable_image_resize_ = node_->get_parameter("enable_image_resize").as_bool(); } template -void RobotReadServiceImpl::DeclareROSParameter( - const std::string &name, - const T& default_val, - const std::string &description) { +void RobotReadServiceImpl::DeclareROSParameter(const std::string& name, + const T& default_val, + const std::string& description) { auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; param_desc.description = description; node_->declare_parameter(name, default_val, param_desc); const auto p = node_->get_parameter(name); - RCLCPP_INFO(node_->get_logger(), "> %s := %s", - name.c_str(), - p.value_to_string().c_str()); + RCLCPP_INFO(node_->get_logger(), "> %s := %s", name.c_str(), p.value_to_string().c_str()); } -void RobotReadServiceImpl::PoseSubCb( - const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - std::lock_guard lock(pose_mutex_); - pose_data_ = msg->pose; +void RobotReadServiceImpl::PoseSubCb(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + std::lock_guard lock(pose_mutex_); + pose_data_ = msg->pose; } -void RobotReadServiceImpl::WrenchSubCb( - const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { - std::lock_guard lock(wrench_mutex_); - wrench_data_ = msg->wrench; +void RobotReadServiceImpl::WrenchSubCb(const geometry_msgs::msg::WrenchStamped::SharedPtr msg) { + std::lock_guard lock(wrench_mutex_); + wrench_data_ = msg->wrench; } -void RobotReadServiceImpl::JointStateSubCb( - const sensor_msgs::msg::JointState::SharedPtr msg) { - std::lock_guard lock(joint_state_mutex_); - joint_state_data_ = *msg; +void RobotReadServiceImpl::JointStateSubCb(const sensor_msgs::msg::JointState::SharedPtr msg) { + std::lock_guard lock(joint_state_mutex_); + joint_state_data_ = *msg; } -void RobotReadServiceImpl::ImageSceneSubCb( - const sensor_msgs::msg::Image::SharedPtr msg) { - std::lock_guard lock(image_scene_mutex_); - image_scene_data_ = *msg; +void RobotReadServiceImpl::ImageSceneSubCb(const sensor_msgs::msg::Image::SharedPtr msg) { + std::lock_guard lock(image_scene_mutex_); + image_scene_data_ = *msg; } -void RobotReadServiceImpl::ImageRightSubCb( - const sensor_msgs::msg::Image::SharedPtr msg) { - std::lock_guard lock(image_right_mutex_); - image_right_data_ = *msg; +void RobotReadServiceImpl::ImageRightSubCb(const sensor_msgs::msg::Image::SharedPtr msg) { + std::lock_guard lock(image_right_mutex_); + image_right_data_ = *msg; } -void RobotReadServiceImpl::ImageLeftSubCb( - const sensor_msgs::msg::Image::SharedPtr msg) { - std::lock_guard lock(image_left_mutex_); - image_left_data_ = *msg; +void RobotReadServiceImpl::ImageLeftSubCb(const sensor_msgs::msg::Image::SharedPtr msg) { + std::lock_guard lock(image_left_mutex_); + image_left_data_ = *msg; } -grpc::Status RobotReadServiceImpl::GetTCPPose( - grpc::ServerContext *context, - const google::protobuf::Empty *request, - proto_aegis_grpc::v1::Pose *response) { - (void) context; - (void) request; - { - std::lock_guard lock(pose_mutex_); - FillProtoPose(pose_data_, response); - } - return grpc::Status::OK; +grpc::Status RobotReadServiceImpl::GetTCPPose(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::Pose* response) { + (void)context; + (void)request; + { + std::lock_guard lock(pose_mutex_); + FillProtoPose(pose_data_, response); + } + return grpc::Status::OK; } -grpc::Status RobotReadServiceImpl::GetWrench( - grpc::ServerContext *context, - const google::protobuf::Empty *request, - proto_aegis_grpc::v1::Wrench *response) { - (void) context; - (void) request; - { - std::lock_guard lock(wrench_mutex_); - FillProtoWrench(wrench_data_, response); - } - return grpc::Status::OK; +grpc::Status RobotReadServiceImpl::GetWrench(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::Wrench* response) { + (void)context; + (void)request; + { + std::lock_guard lock(wrench_mutex_); + FillProtoWrench(wrench_data_, response); + } + return grpc::Status::OK; } -grpc::Status RobotReadServiceImpl::GetJointStates( - grpc::ServerContext *context, const google::protobuf::Empty *request, - proto_aegis_grpc::v1::JointState *response) { - (void) context; - (void) request; - { - std::lock_guard lock(joint_state_mutex_); - FillProtoJointState(joint_state_data_, response); - } - return grpc::Status::OK; +grpc::Status RobotReadServiceImpl::GetJointStates(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::JointState* response) { + (void)context; + (void)request; + { + std::lock_guard lock(joint_state_mutex_); + FillProtoJointState(joint_state_data_, response); + } + return grpc::Status::OK; } -grpc::Status RobotReadServiceImpl::GetCameraSceneImage( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::Image* response) { - (void)context; - (void)request; - { - std::lock_guard lock(image_scene_mutex_); - FillProtoImage(image_scene_data_, response, scene_resized_); - } - return grpc::Status::OK; +grpc::Status RobotReadServiceImpl::GetCameraSceneImage(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::Image* response) { + (void)context; + (void)request; + { + std::lock_guard lock(image_scene_mutex_); + FillProtoImage(image_scene_data_, response, scene_resized_); + } + return grpc::Status::OK; } -grpc::Status RobotReadServiceImpl::GetCameraRightImage( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::Image* response) { - (void)context; - (void)request; - { - std::lock_guard lock(image_right_mutex_); - FillProtoImage(image_right_data_, response, right_resized_); - } - return grpc::Status::OK; +grpc::Status RobotReadServiceImpl::GetCameraRightImage(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::Image* response) { + (void)context; + (void)request; + { + std::lock_guard lock(image_right_mutex_); + FillProtoImage(image_right_data_, response, right_resized_); + } + return grpc::Status::OK; } -grpc::Status RobotReadServiceImpl::GetCameraLeftImage( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::Image* response) { - (void)context; - (void)request; - { - std::lock_guard lock(image_left_mutex_); - FillProtoImage(image_left_data_, response, left_resized_); - } - return grpc::Status::OK; +grpc::Status RobotReadServiceImpl::GetCameraLeftImage(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::Image* response) { + (void)context; + (void)request; + { + std::lock_guard lock(image_left_mutex_); + FillProtoImage(image_left_data_, response, left_resized_); + } + return grpc::Status::OK; } -grpc::Status RobotReadServiceImpl::GetRobotState( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::RobotState* response) { - // TODO(issue#85): Guard the freshness of the data in gRPC server - (void)context; - (void)request; - { +grpc::Status RobotReadServiceImpl::GetRobotState(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::RobotState* response) { + // TODO(issue#85): Guard the freshness of the data in gRPC server + (void)context; + (void)request; + { std::lock_guard lock(pose_mutex_); FillProtoPose(pose_data_, response->mutable_pose()); - } - { + } + { std::lock_guard lock(wrench_mutex_); FillProtoWrench(wrench_data_, response->mutable_wrench()); - } - { + } + { std::lock_guard lock(joint_state_mutex_); FillProtoJointState(joint_state_data_, response->mutable_joint_state()); - } - return grpc::Status::OK; + } + return grpc::Status::OK; } - -grpc::Status RobotReadServiceImpl::GetRobotVision( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::RobotVision* response) { - // TODO(issue#85): Guard the freshness of the data in gRPC server - (void) context; - (void) request; - { +grpc::Status RobotReadServiceImpl::GetRobotVision(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::RobotVision* response) { + // TODO(issue#85): Guard the freshness of the data in gRPC server + (void)context; + (void)request; + { std::lock_guard lock(image_scene_mutex_); FillProtoImage(image_scene_data_, response->mutable_image_scene(), scene_resized_); - } - { + } + { std::lock_guard lock(image_right_mutex_); FillProtoImage(image_right_data_, response->mutable_image_right(), right_resized_); - } - { + } + { std::lock_guard lock(image_left_mutex_); FillProtoImage(image_left_data_, response->mutable_image_left(), left_resized_); - } - return grpc::Status::OK; + } + return grpc::Status::OK; } -grpc::Status RobotReadServiceImpl::GetAll( - grpc::ServerContext* context, - const google::protobuf::Empty* request, - proto_aegis_grpc::v1::RobotObservation* response) { - (void) context; - (void) request; - { - std::lock_guard lock(pose_mutex_); - FillProtoPose(pose_data_, response->mutable_robot_state()->mutable_pose()); - } - { - std::lock_guard lock(wrench_mutex_); - FillProtoWrench(wrench_data_, response->mutable_robot_state()->mutable_wrench()); - } - { - std::lock_guard lock(joint_state_mutex_); - FillProtoJointState(joint_state_data_, response->mutable_robot_state()->mutable_joint_state()); - } - { - std::lock_guard lock(image_scene_mutex_); - FillProtoImage(image_scene_data_, response->mutable_robot_vision()->mutable_image_scene(), scene_resized_); - } - { - std::lock_guard lock(image_right_mutex_); - FillProtoImage(image_right_data_, response->mutable_robot_vision()->mutable_image_right(), right_resized_); - } - { - std::lock_guard lock(image_left_mutex_); - FillProtoImage(image_left_data_, response->mutable_robot_vision()->mutable_image_left(), left_resized_); - } - return grpc::Status::OK; +grpc::Status RobotReadServiceImpl::GetAll(grpc::ServerContext* context, + const google::protobuf::Empty* request, + proto_aegis_grpc::v1::RobotObservation* response) { + (void)context; + (void)request; + { + std::lock_guard lock(pose_mutex_); + FillProtoPose(pose_data_, response->mutable_robot_state()->mutable_pose()); + } + { + std::lock_guard lock(wrench_mutex_); + FillProtoWrench(wrench_data_, response->mutable_robot_state()->mutable_wrench()); + } + { + std::lock_guard lock(joint_state_mutex_); + FillProtoJointState(joint_state_data_, response->mutable_robot_state()->mutable_joint_state()); + } + { + std::lock_guard lock(image_scene_mutex_); + FillProtoImage(image_scene_data_, response->mutable_robot_vision()->mutable_image_scene(), scene_resized_); + } + { + std::lock_guard lock(image_right_mutex_); + FillProtoImage(image_right_data_, response->mutable_robot_vision()->mutable_image_right(), right_resized_); + } + { + std::lock_guard lock(image_left_mutex_); + FillProtoImage(image_left_data_, response->mutable_robot_vision()->mutable_image_left(), left_resized_); + } + return grpc::Status::OK; } -void RobotReadServiceImpl::FillProtoPose( - const geometry_msgs::msg::Pose& ros, - proto_aegis_grpc::v1::Pose* out) { +void RobotReadServiceImpl::FillProtoPose(const geometry_msgs::msg::Pose& ros, proto_aegis_grpc::v1::Pose* out) { auto* pos = out->mutable_position(); - pos->set_x(ros.position.x); - pos->set_y(ros.position.y); - pos->set_z(ros.position.z); - - auto* ori = out->mutable_orientation(); - ori->set_x(ros.orientation.x); - ori->set_y(ros.orientation.y); - ori->set_z(ros.orientation.z); - ori->set_w(ros.orientation.w); + pos->set_x(ros.position.x); + pos->set_y(ros.position.y); + pos->set_z(ros.position.z); + + auto* ori = out->mutable_orientation(); + ori->set_x(ros.orientation.x); + ori->set_y(ros.orientation.y); + ori->set_z(ros.orientation.z); + ori->set_w(ros.orientation.w); } -void RobotReadServiceImpl::FillProtoWrench( - const geometry_msgs::msg::Wrench& ros, - proto_aegis_grpc::v1::Wrench* out) { - auto* f = out->mutable_force(); - f->set_x(ros.force.x); - f->set_y(ros.force.y); - f->set_z(ros.force.z); - - auto* t = out->mutable_torque(); - t->set_x(ros.torque.x); - t->set_y(ros.torque.y); - t->set_z(ros.torque.z); +void RobotReadServiceImpl::FillProtoWrench(const geometry_msgs::msg::Wrench& ros, proto_aegis_grpc::v1::Wrench* out) { + auto* f = out->mutable_force(); + f->set_x(ros.force.x); + f->set_y(ros.force.y); + f->set_z(ros.force.z); + + auto* t = out->mutable_torque(); + t->set_x(ros.torque.x); + t->set_y(ros.torque.y); + t->set_z(ros.torque.z); } -void RobotReadServiceImpl::FillProtoJointState( - const sensor_msgs::msg::JointState& ros, - proto_aegis_grpc::v1::JointState* out) { - out->clear_name(); - out->clear_position(); - out->clear_velocity(); - out->clear_effort(); - - for (const auto& v : ros.name) out->add_name(v); - for (const auto& v : ros.position) out->add_position(v); - for (const auto& v : ros.velocity) out->add_velocity(v); - for (const auto& v : ros.effort) out->add_effort(v); +void RobotReadServiceImpl::FillProtoJointState(const sensor_msgs::msg::JointState& ros, + proto_aegis_grpc::v1::JointState* out) { + out->clear_name(); + out->clear_position(); + out->clear_velocity(); + out->clear_effort(); + + for (const auto& v : ros.name) + out->add_name(v); + for (const auto& v : ros.position) + out->add_position(v); + for (const auto& v : ros.velocity) + out->add_velocity(v); + for (const auto& v : ros.effort) + out->add_effort(v); } - void RobotReadServiceImpl::FillProtoImage( - const sensor_msgs::msg::Image& ros, - proto_aegis_grpc::v1::Image* out, - cv::Mat& buffer) - { - assert((ros.encoding == "bgr8" && "Unsupported image encoding (expected BGR8).")); - if (!enable_image_resize_) { - out->set_height(ros.height); - out->set_width(ros.width); - out->set_step(ros.step); - out->set_data( - const_cast(ros.data.data()), - static_cast(ros.height * ros.step) - ); - return; - } - - const cv::Mat src( - ros.height, - ros.width, - CV_8UC3, - const_cast(ros.data.data()), - ros.step - ); - - cv::resize( - src, - buffer, - cv::Size(target_img_width_, target_img_height_), - 0, 0, cv::INTER_LINEAR - ); - - out->set_height(buffer.rows); - out->set_width(buffer.cols); - out->set_step(buffer.step); - out->set_data(buffer.data, buffer.total() * buffer.elemSize()); +void RobotReadServiceImpl::FillProtoImage(const sensor_msgs::msg::Image& ros, + proto_aegis_grpc::v1::Image* out, + cv::Mat& buffer) { + assert((ros.encoding == "bgr8" && "Unsupported image encoding (expected BGR8).")); + if (!enable_image_resize_) { + out->set_height(ros.height); + out->set_width(ros.width); + out->set_step(ros.step); + out->set_data(const_cast(ros.data.data()), static_cast(ros.height * ros.step)); + return; } + const cv::Mat src(ros.height, ros.width, CV_8UC3, const_cast(ros.data.data()), ros.step); + + cv::resize(src, buffer, cv::Size(target_img_width_, target_img_height_), 0, 0, cv::INTER_LINEAR); + + out->set_height(buffer.rows); + out->set_width(buffer.cols); + out->set_step(buffer.step); + out->set_data(buffer.data, buffer.total() * buffer.elemSize()); +} -} // namespace aegis_grpc +} // namespace aegis_grpc