Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions .clang-format
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
BasedOnStyle: Chromium
SortIncludes: false
ColumnLimit: 120
16 changes: 8 additions & 8 deletions .pre-commit-config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand All @@ -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"
Expand Down
187 changes: 85 additions & 102 deletions aegis_grpc/include/aegis_grpc/robot_control_service.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,110 +18,93 @@
#include <moveit/move_group_interface/move_group_interface.h>
#include <std_srvs/srv/trigger.hpp>


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<GripperCommand>;

enum class ServoMode {
None,
JointJog,
TCPTwist
};

explicit RobotControlServiceImpl(std::shared_ptr<rclcpp::Node> 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<std::string>& activate,
const std::vector<std::string>& deactivate);
bool CallServoStartService();
bool CallServoStopService();

void ServoPublishLoop();
void GripperSendGoal(double position, double max_effort);

rclcpp::Logger get_logger() const;

template <class T>
void DeclareROSParameter(const std::string& name, const T& default_val, const std::string& description);

std::shared_ptr<rclcpp::Node> node_;
std::unique_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;

rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_controller_client_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr start_servo_client_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr stop_servo_client_;
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr servo_joint_pub_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr servo_tcp_pub_;
rclcpp_action::Client<GripperCommand>::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<bool> gripper_in_use_;
std::chrono::duration<double> action_timeout_;
std::string gripper_cmd_msg_;
bool gripper_cmd_success_;
std::atomic<bool> 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<GripperCommand>;

enum class ServoMode { None, JointJog, TCPTwist };

explicit RobotControlServiceImpl(std::shared_ptr<rclcpp::Node> 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<std::string>& activate, const std::vector<std::string>& deactivate);
bool CallServoStartService();
bool CallServoStopService();

void ServoPublishLoop();
void GripperSendGoal(double position, double max_effort);

rclcpp::Logger get_logger() const;

template <class T>
void DeclareROSParameter(const std::string& name, const T& default_val, const std::string& description);

std::shared_ptr<rclcpp::Node> node_;
std::unique_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;

rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_controller_client_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr start_servo_client_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr stop_servo_client_;
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr servo_joint_pub_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr servo_tcp_pub_;
rclcpp_action::Client<GripperCommand>::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<bool> gripper_in_use_;
std::chrono::duration<double> action_timeout_;
std::string gripper_cmd_msg_;
bool gripper_cmd_success_;
std::atomic<bool> gripper_cmd_done_;
};

} // namespace aegis_grpc
} // namespace aegis_grpc
#endif // AEGIS_GRPC__ROBOT_CONTROL_SERVICE_HPP_
Loading