Skip to content

Commit fbc5a83

Browse files
macmacalmacmacal
authored
Merge pull request #89 from AGH-CEAI/feature/add_grpc_moveit
Enable gRPC-based robot motion commands with MoveIt
2 parents 0b75760 + 1599a8c commit fbc5a83

File tree

8 files changed

+198
-75
lines changed

8 files changed

+198
-75
lines changed

‎aegis_grpc/CHANGELOG.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
99

1010
### Added
1111

12+
* [PR-88](https://github.com/AGH-CEAI/aegis_ros/pull/88) - Added gRPC robot motion commands with MoveIt.
1213
* [PR-88](https://github.com/AGH-CEAI/aegis_ros/pull/88) - MVP of the gRPC Python client.
1314
* [PR-82](https://github.com/AGH-CEAI/aegis_ros/pull/82) - MVP of the gRPC-ROS server.
1415

‎aegis_grpc/CMakeLists.txt‎

Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -14,20 +14,22 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
1414
# Dependencies
1515

1616
find_package(ament_cmake REQUIRED)
17-
find_package(rclcpp REQUIRED)
18-
find_package(rclcpp_action REQUIRED)
19-
find_package(control_msgs REQUIRED)
20-
find_package(geometry_msgs REQUIRED)
21-
find_package(sensor_msgs REQUIRED)
2217

2318
set(ROS_DEPENDS
2419
rclcpp
2520
rclcpp_action
2621
control_msgs
2722
geometry_msgs
28-
sensor_msgs)
23+
sensor_msgs
24+
moveit_msgs
25+
moveit_ros_planning_interface)
2926

30-
find_program(GRPC_CPP_PLUGIN_EXECUTABLE grpc_cpp_plugin REQUIRED)
27+
# Hide warnings from importing moveit packages
28+
cmake_policy(SET CMP0167 NEW)
29+
30+
foreach(ros_dep ${ROS_DEPENDS})
31+
find_package(${ros_dep} REQUIRED)
32+
endforeach()
3133

3234
# gRPC package for Ubuntu 22.04 (ROS Humble) can't be find by CMake's find_package()
3335
# https://bugs.launchpad.net/ubuntu/+source/grpc/+bug/1935709

‎aegis_grpc/README.md‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -104,12 +104,12 @@ The server is split into 2 services defined in [`proto_aegis_grpc.v1.robot_srvs`
104104

105105
The "ROS-getters" are implemented in the [`RobotReadServiceImpl`](./include/aegis_grpc/robot_read_service.hpp) class as the following methods:
106106

107-
| Method name | Desc. | Impl. | gRPC Request | gRPC Response |
108-
| ---------------------------------------------------- | --------------------------- | ----- | ----------------------- | --------------------------------------------------------------------------------------- |
109-
| `proto_aegis_grpc.v1.RobotReadService.GetAll` | Get the all available data. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.robot_srvs.RobotState`](./proto_aegis_grpc/v1/robot_srvs.proto) |
110-
| `proto_aegis_grpc.v1.RobotReadService.GetJointState` | Get the joints' states. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.sensor_msgs.JointState`](./proto_aegis_grpc/v1/sensor_msgs.proto) |
111-
| `proto_aegis_grpc.v1.RobotReadService.GetTCPPose` | Get the TCP pose. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.geometry_msgs.Pose`](./proto_aegis_grpc/v1/geometry_msgs.proto) |
112-
| `proto_aegis_grpc.v1.RobotReadService.GetWrench` | Read force/torque sensor. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.geometry_msgs.Wrench`](./proto_aegis_grpc/v1/geometry_msgs.proto) |
107+
| Method name | Desc. | Impl. | gRPC Request | gRPC Response |
108+
|-------------------------------------------------------|-----------------------------|-------|-------------------------|-----------------------------------------------------------------------------------------|
109+
| `proto_aegis_grpc.v1.RobotReadService.GetAll` | Get the all available data. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.robot_srvs.RobotState`](./proto_aegis_grpc/v1/robot_srvs.proto) |
110+
| `proto_aegis_grpc.v1.RobotReadService.GetJointStates` | Get the joints' states. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.sensor_msgs.JointState`](./proto_aegis_grpc/v1/sensor_msgs.proto) |
111+
| `proto_aegis_grpc.v1.RobotReadService.GetTCPPose` | Get the TCP pose. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.geometry_msgs.Pose`](./proto_aegis_grpc/v1/geometry_msgs.proto) |
112+
| `proto_aegis_grpc.v1.RobotReadService.GetWrench` | Read force/torque sensor. | ✅ | `google.protobuf.Empty` | [`proto_aegis_grpc.v1.geometry_msgs.Wrench`](./proto_aegis_grpc/v1/geometry_msgs.proto) |
113113

114114
You can always list the methods with the `grpcurl` command:
115115
```bash

‎aegis_grpc/include/aegis_grpc/robot_control_service.hpp‎

Lines changed: 21 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,8 @@
1313
#include <control_msgs/msg/joint_jog.hpp>
1414
#include <geometry_msgs/msg/twist.hpp>
1515
#include <geometry_msgs/msg/twist_stamped.hpp>
16+
#include <geometry_msgs/msg/pose.hpp>
17+
#include <moveit/move_group_interface/move_group_interface.h>
1618

1719

1820
namespace aegis_grpc {
@@ -34,39 +36,38 @@ class RobotControlServiceImpl final
3436
// TODO(issue#87) Create additional methods to enable and disable servo control.
3537
grpc::Status ServoJoint(
3638
grpc::ServerContext* context,
37-
const proto_aegis_grpc::v1::JointJog *request,
38-
google::protobuf::Empty *response) override;
39+
const proto_aegis_grpc::v1::JointJog* request,
40+
google::protobuf::Empty* response) override;
3941

4042
grpc::Status ServoTCP(
4143
grpc::ServerContext* context,
42-
const proto_aegis_grpc::v1::Twist *request,
43-
google::protobuf::Empty *response) override;
44+
const proto_aegis_grpc::v1::Twist* request,
45+
google::protobuf::Empty* response) override;
4446

45-
// TODO(issue#83) add MoveIt2 actions to plan&execute trajectories to targets in Joints and Poses.
46-
// grpc::Status GotoPose(
47-
// grpc::ServerContext* context,
48-
// const proto_aegis_grpc::v1::Pose *request,
49-
// proto_aegis_grpc::v1::TriggerResponse *response) override;
47+
grpc::Status GotoPose(
48+
grpc::ServerContext* context,
49+
const proto_aegis_grpc::v1::Pose* request,
50+
proto_aegis_grpc::v1::TriggerResponse* response) override;
5051

51-
// grpc::Status GotoJoints(
52-
// grpc::ServerContext* context,
53-
// const proto_aegis_grpc::v1::JointState *request,
54-
// proto_aegis_grpc::v1::TriggerResponse *response) override;
52+
grpc::Status GotoJoints(
53+
grpc::ServerContext* context,
54+
const proto_aegis_grpc::v1::JointState* request,
55+
proto_aegis_grpc::v1::TriggerResponse* response) override;
5556

5657
grpc::Status GripperSetPosition(
5758
grpc::ServerContext* context,
58-
const proto_aegis_grpc::v1::GripperSetPositionRequest *request,
59-
proto_aegis_grpc::v1::TriggerResponse *response) override;
59+
const proto_aegis_grpc::v1::GripperSetPositionRequest* request,
60+
proto_aegis_grpc::v1::TriggerResponse* response) override;
6061

6162
grpc::Status GripperClose(
6263
grpc::ServerContext* context,
63-
const google::protobuf::Empty *request,
64-
proto_aegis_grpc::v1::TriggerResponse *response) override;
64+
const google::protobuf::Empty* request,
65+
proto_aegis_grpc::v1::TriggerResponse* response) override;
6566

6667
grpc::Status GripperOpen(
6768
grpc::ServerContext* context,
68-
const google::protobuf::Empty *request,
69-
proto_aegis_grpc::v1::TriggerResponse *response) override;
69+
const google::protobuf::Empty* request,
70+
proto_aegis_grpc::v1::TriggerResponse* response) override;
7071

7172
private:
7273
rclcpp::Logger get_logger() const;
@@ -78,6 +79,7 @@ class RobotControlServiceImpl final
7879
void GripperSendGoal(double position, double max_effort);
7980

8081
std::shared_ptr<rclcpp::Node> node_;
82+
std::unique_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
8183

8284
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr servo_joint_pub_;
8385
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr servo_tcp_pub_;

‎aegis_grpc/include/aegis_grpc/robot_read_service.hpp‎

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -20,22 +20,22 @@ class RobotReadServiceImpl final
2020
public:
2121
explicit RobotReadServiceImpl(std::shared_ptr<rclcpp::Node> node);
2222

23-
grpc::Status GetTCPPose(grpc::ServerContext *context,
24-
const google::protobuf::Empty *request,
25-
proto_aegis_grpc::v1::Pose *response) override;
23+
grpc::Status GetTCPPose(grpc::ServerContext* context,
24+
const google::protobuf::Empty* request,
25+
proto_aegis_grpc::v1::Pose* response) override;
2626

27-
grpc::Status GetWrench(grpc::ServerContext *context,
28-
const google::protobuf::Empty *request,
29-
proto_aegis_grpc::v1::Wrench *response) override;
27+
grpc::Status GetWrench(grpc::ServerContext* context,
28+
const google::protobuf::Empty* request,
29+
proto_aegis_grpc::v1::Wrench* response) override;
3030

3131
grpc::Status
32-
GetJointStates(grpc::ServerContext *context,
33-
const google::protobuf::Empty *request,
34-
proto_aegis_grpc::v1::JointState *response) override;
32+
GetJointStates(grpc::ServerContext* context,
33+
const google::protobuf::Empty* request,
34+
proto_aegis_grpc::v1::JointState* response) override;
3535

36-
grpc::Status GetAll(grpc::ServerContext *context,
37-
const google::protobuf::Empty *request,
38-
proto_aegis_grpc::v1::RobotState *response) override;
36+
grpc::Status GetAll(grpc::ServerContext* context,
37+
const google::protobuf::Empty* request,
38+
proto_aegis_grpc::v1::RobotState* response) override;
3939

4040
// TODO(issue#84) implement getters for images from cameras (RGB & RGBD)
4141
// TODO(issue#85) develop synchronization guard to monitor the freshness of the data

‎aegis_grpc/package.xml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,8 @@
1919
<depend>control_msgs</depend>
2020
<depend>geometry_msgs</depend>
2121
<depend>libgrpc</depend>
22+
<depend>moveit_msgs</depend>
23+
<depend>moveit_ros_planning_interface</depend>
2224
<depend>protobuf-compiler-grpc</depend>
2325
<depend>protobuf-dev</depend>
2426
<depend>rclcpp</depend>

‎aegis_grpc/python_client/aegis_grpc_client/grpc_client.py‎

Lines changed: 51 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -236,8 +236,8 @@ async def servo_joint(
236236

237237
async def servo_tcp(
238238
self,
239-
linear: Union[tuple[float, float, float], np.ndarray], # np.array([vx,vy,vz])
240-
angular: Union[tuple[float, float, float], np.ndarray], # np.array([wx,wy,wz])
239+
linear: Union[tuple[float, float, float], np.ndarray],
240+
angular: Union[tuple[float, float, float], np.ndarray],
241241
) -> None:
242242
"""
243243
Perform TCP servo with Cartesian velocity control.
@@ -278,38 +278,52 @@ async def servo_tcp(
278278
self.logger.error(f"ServoTCP failed: {e}")
279279
raise
280280

281-
# TODO(issue#83) add MoveIt2 actions to plan&execute trajectories to targets in Joints and Poses.
282281
async def goto_pose(
283282
self,
284-
position: Union[tuple[float, float, float], np.ndarray], # np.array([vx,vy,vz])
285-
orientation: Union[
286-
tuple[float, float, float], np.ndarray
287-
], # np.array([wx,wy,wz])
283+
position: Union[tuple[float, float, float], np.ndarray],
284+
orientation: Union[tuple[float, float, float, float], np.ndarray],
288285
) -> tuple[bool, str]:
289286
"""
290287
Move robot to specified TCP pose.
291288
292289
Args:
293-
pose: Target Pose with position and orientation
290+
pose: Target Pose with position and orientation (quaternion)
294291
295292
Returns:
296293
(success, message) tuple
297294
"""
298-
raise NotImplementedError
299-
# TODO(issue#83) construct geometry_msgs_pb2.Pose message
300-
# self._check_connected()
301-
# try:
302-
# response = await self.control_stub.GotoPose(pose)
303-
# return response.success, response.msg
304-
# except grpc.RpcError as e:
305-
# self.logger.error(f"GotoPose failed: {e}")
306-
# raise
307-
308-
# TODO(issue#83) add MoveIt2 actions to plan&execute trajectories to targets in Joints and Poses.
295+
self._check_connected()
296+
297+
if isinstance(position, np.ndarray):
298+
position = tuple(position)
299+
if isinstance(orientation, np.ndarray):
300+
orientation = tuple(orientation)
301+
302+
pose = geometry_msgs_pb2.Pose(
303+
position=geometry_msgs_pb2.Point(
304+
x=position[0],
305+
y=position[1],
306+
z=position[2],
307+
),
308+
orientation=geometry_msgs_pb2.Quaternion(
309+
x=orientation[0],
310+
y=orientation[1],
311+
z=orientation[2],
312+
w=orientation[3],
313+
),
314+
)
315+
316+
try:
317+
response = await self.control_stub.GotoPose(pose)
318+
return response.success, response.msg
319+
except grpc.RpcError as e:
320+
self.logger.error(f"Failed to send robot motion command: {e}")
321+
raise
322+
309323
async def goto_joints(
310324
self,
311-
names: list[str],
312-
positions: Optional[Union[list[float], np.ndarray]] = None,
325+
names: tuple[str],
326+
positions: Union[tuple[float], np.ndarray],
313327
) -> tuple[bool, str]:
314328
"""
315329
Move robot to specified joint configuration.
@@ -320,15 +334,22 @@ async def goto_joints(
320334
Returns:
321335
(success, message) tuple
322336
"""
323-
raise NotImplementedError
324-
# TODO(issue#83) construct sensor_msgs_pb2.JointState message
325-
# self._check_connected()
326-
# try:
327-
# response = await self.control_stub.GotoJoints(joint_state)
328-
# return response.success, response.msg
329-
# except grpc.RpcError as e:
330-
# self.logger.error(f"GotoJoints failed: {e}")
331-
# raise
337+
self._check_connected()
338+
339+
if isinstance(positions, np.ndarray):
340+
positions = tuple(positions)
341+
342+
joint_state = sensor_msgs_pb2.JointState(
343+
name=names,
344+
position=positions,
345+
)
346+
347+
try:
348+
response = await self.control_stub.GotoJoints(joint_state)
349+
return response.success, response.msg
350+
except grpc.RpcError as e:
351+
self.logger.error(f"Failed to send robot motion command: {e}")
352+
raise
332353

333354
async def gripper_set_position(
334355
self, position: float, effort: float

0 commit comments

Comments
 (0)