Added services to change mode & configs

Signed-off-by: Juancams <[email protected]>

go2_driver/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(nav_msgs REQUIRED)
 find_package(unitree_go REQUIRED)
 find_package(unitree_api REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(go2_interfaces REQUIRED)
 
 set(dependencies
   rclcpp
@@ -26,6 +27,7 @@ set(dependencies
   unitree_go
   unitree_api
   tf2_ros
+  go2_interfaces
 )
 
 include_directories(include)

go2_driver/include/go2_driver/go2_api_id.hpp

@@ -0,0 +1,79 @@
+// BSD 3-Clause License
+
+// Copyright (c) 2024, Intelligent Robotics Lab
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+
+// * Redistributions of source code must retain the above copyright notice, this
+//   list of conditions and the following disclaimer.
+
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+
+// * Neither the name of the copyright holder nor the names of its
+//   contributors may be used to endorse or promote products derived from
+//   this software without specific prior written permission.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef GO2_DRIVER__GO2_API_ID_HPP
+#define GO2_DRIVER__GO2_API_ID_HPP
+
+namespace go2_driver
+{
+
+enum class Mode
+{
+  Damp = 1001,
+  BalanceStand = 1002,
+  StopMove = 1003,
+  StandUp = 1004,
+  StandDown = 1005,
+  RecoveryStand = 1006,
+  Euler = 1007,
+  Move = 1008,
+  Sit = 1009,
+  RiseSit = 1010,
+  SwitchGait = 1011,
+  Trigger = 1012,
+  BodyHeight = 1013,
+  FootRaiseHeight = 1014,
+  SpeedLevel = 1015,
+  Hello = 1016,
+  Stretch = 1017,
+  TrajectoryFollow = 1018,
+  ContinuousGait = 1019,
+  Content = 1020,
+  Wallow = 1021,
+  Dance1 = 1022,
+  Dance2 = 1023,
+  GetBodyHeight = 1024,
+  GetFootRaiseHeight = 1025,
+  GetSpeedLevel = 1026,
+  SwitchJoystick = 1027,
+  Pose = 1028,
+  Scrape = 1029,
+  FrontFlip = 1030,
+  FrontJump = 1031,
+  FrontPounce = 1032,
+  WiggleHips = 1033,
+  GetState = 1034,
+  EconomicGait = 1035,
+  FingerHeart = 1036,
+};
+
+}  // namespace go2_driver
+
+#endif // GO2_DRIVER__GO2_API_ID_HPP

go2_driver/include/go2_driver/go2_driver.hpp

@@ -39,12 +39,21 @@
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/joy.hpp>
-#include "unitree_go/msg/go2_state.hpp"
 #include "unitree_go/msg/low_state.hpp"
 #include "unitree_go/msg/imu_state.hpp"
 #include "tf2_ros/transform_broadcaster.h"
 #include "nlohmann/json.hpp"
 #include "unitree_api/msg/request.hpp"
+#include "go2_driver/go2_api_id.hpp"
+#include "go2_interfaces/srv/body_height.hpp"
+#include "go2_interfaces/srv/continuous_gait.hpp"
+#include "go2_interfaces/srv/euler.hpp"
+#include "go2_interfaces/srv/foot_raise_height.hpp"
+#include "go2_interfaces/srv/mode.hpp"
+#include "go2_interfaces/srv/pose.hpp"
+#include "go2_interfaces/srv/speed_level.hpp"
+#include "go2_interfaces/srv/switch_gait.hpp"
+#include "go2_interfaces/srv/switch_joystick.hpp"
 
 namespace go2_driver
 {
@@ -61,6 +70,50 @@ class Go2Driver : public rclcpp::Node
   void publish_joint_states(const unitree_go::msg::LowState::SharedPtr msg);
   void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
 
+  void handleBodyHeight(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<go2_interfaces::srv::BodyHeight::Request> request,
+    const std::shared_ptr<go2_interfaces::srv::BodyHeight::Response> response);
+
+  void handleContinuousGait(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<go2_interfaces::srv::ContinuousGait::Request> request,
+    const std::shared_ptr<go2_interfaces::srv::ContinuousGait::Response> response);
+
+  void handleEuler(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<go2_interfaces::srv::Euler::Request> request,
+    const std::shared_ptr<go2_interfaces::srv::Euler::Response> response);
+
+  void handleFootRaiseHeight(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<go2_interfaces::srv::FootRaiseHeight::Request> request,
+    const std::shared_ptr<go2_interfaces::srv::FootRaiseHeight::Response> response);
+
+  void handleMode(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<go2_interfaces::srv::Mode::Request> request,
+    const std::shared_ptr<go2_interfaces::srv::Mode::Response> response);
+
+  void handlePose(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<go2_interfaces::srv::Pose::Request> request,
+    const std::shared_ptr<go2_interfaces::srv::Pose::Response> response);
+
+  void handleSpeedLevel(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<go2_interfaces::srv::SpeedLevel::Request> request,
+    const std::shared_ptr<go2_interfaces::srv::SpeedLevel::Response> response);
+
+  void handleSwitchGait(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<go2_interfaces::srv::SwitchGait::Request> request,
+    const std::shared_ptr<go2_interfaces::srv::SwitchGait::Response> response);
+
+  void handleSwitchJoystick(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<go2_interfaces::srv::SwitchJoystick::Request> request,
+    const std::shared_ptr<go2_interfaces::srv::SwitchJoystick::Response> response);
 
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr robot_pose_sub_;
@@ -70,15 +123,26 @@ class Go2Driver : public rclcpp::Node
 
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_pub_;
-  rclcpp::Publisher<unitree_go::msg::Go2State>::SharedPtr go2_state_pub_;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
   rclcpp::Publisher<unitree_go::msg::IMUState>::SharedPtr imu_pub_;
   rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr request_pub_;
 
+  rclcpp::Service<go2_interfaces::srv::BodyHeight>::SharedPtr set_body_height_service_;
+  rclcpp::Service<go2_interfaces::srv::ContinuousGait>::SharedPtr set_continuous_gait_service_;
+  rclcpp::Service<go2_interfaces::srv::Euler>::SharedPtr set_euler_service_;
+  rclcpp::Service<go2_interfaces::srv::FootRaiseHeight>::SharedPtr set_foot_raise_height_service_;
+  rclcpp::Service<go2_interfaces::srv::Mode>::SharedPtr set_mode_service_;
+  rclcpp::Service<go2_interfaces::srv::Pose>::SharedPtr set_pose_service_;
+  rclcpp::Service<go2_interfaces::srv::SpeedLevel>::SharedPtr set_speed_level_service_;
+  rclcpp::Service<go2_interfaces::srv::SwitchGait>::SharedPtr set_switch_gait_service_;
+  rclcpp::Service<go2_interfaces::srv::SwitchJoystick>::SharedPtr set_switch_joystick_service_;
+
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::TimerBase::SharedPtr timer_lidar_;
   tf2_ros::TransformBroadcaster tf_broadcaster_;
   sensor_msgs::msg::Joy joy_state_;
+
+  bool odom_published_{false};
 };
 
 }  // namespace go2_driver

go2_driver/launch/go2_driver.launch.py

@@ -40,7 +40,7 @@ def generate_launch_description():
     composable_node = ComposableNode(
         package='go2_driver',
         plugin='go2_driver::Go2Driver',
-        name='test_node',
+        name='go2_driver',
         namespace='',
 
     )

go2_driver/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
   <name>go2_driver</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>GO2 robot driver package</description>
   <maintainer email="[email protected]">Juan Carlos Manzanares Serrano</maintainer>
   <license>BSD 3-Clause License</license>
 
@@ -18,6 +18,7 @@
   <depend>unitree_go</depend>
   <depend>unitree_api</depend>
   <depend>tf2_ros</depend>
+  <depend>go2_interfaces</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>