diff --git a/.github/workflows/humble.yaml b/.github/workflows/humble.yaml index 5ebc51e..79d738c 100644 --- a/.github/workflows/humble.yaml +++ b/.github/workflows/humble.yaml @@ -20,7 +20,7 @@ jobs: - name: Build ROS 2 package uses: ros-tooling/action-ros-ci@v0.3 with: - package-name: go2_robot go2_description go2_interfaces + package-name: go2_robot go2_description go2_bringup go2_driver go2_hardware go2_simulation unitree_api unitree_go go2_interfaces go2_nav go2_rviz target-ros2-distro: humble colcon-defaults: | { diff --git a/.github/workflows/humble_devel.yaml b/.github/workflows/humble_devel.yaml index 8a04fba..4873282 100644 --- a/.github/workflows/humble_devel.yaml +++ b/.github/workflows/humble_devel.yaml @@ -12,6 +12,9 @@ jobs: - name: Checkout uses: actions/checkout@v2 + - name: Install nlohmann + run: sudo apt -y install nlohmann-json3-dev + - name: Setup ROS 2 environment uses: ros-tooling/setup-ros@0.7.0 with: @@ -20,7 +23,7 @@ jobs: - name: Build ROS 2 package uses: ros-tooling/action-ros-ci@v0.3 with: - package-name: go2_robot go2_description go2_interfaces + package-name: go2_robot go2_description go2_bringup go2_driver go2_hardware go2_simulation unitree_api unitree_go go2_interfaces go2_nav go2_rviz target-ros2-distro: humble colcon-defaults: | { diff --git a/README.md b/README.md index 558c725..9fd926b 100644 --- a/README.md +++ b/README.md @@ -1 +1,218 @@ -# Unitree GO2 Robot +# Unitree GO2 Robot ROS 2 + +

+Go2 point cloud +

+ +[![License](https://img.shields.io/badge/license-BSD--3-yellow.svg)](https://opensource.org/licenses/BSD-3-Clause) +![distro](https://img.shields.io/badge/Ubuntu%2022-Jammy%20Jellyfish-green) +![distro](https://img.shields.io/badge/ROS2-Humble-blue) +[![humble](https://github.com/IntelligentRoboticsLabs/go2_robot/actions/workflows/humble.yaml/badge.svg)](https://github.com/IntelligentRoboticsLabs/go2_robot/actions/workflows/humble.yaml) +[![humble-devel](https://github.com/IntelligentRoboticsLabs/go2_robot/actions/workflows/humble_devel.yaml/badge.svg)](https://github.com/IntelligentRoboticsLabs/go2_robot/actions/workflows/humble_devel.yaml) + +In this package is our integration for the Unitee Go2 robot. + +## Checklist + +- [x] robot description +- [x] odom +- [x] pointcloud +- [x] joint_states +- [x] Visualization in rviz +- [x] cmd_vel +- [x] go2_interfaces +- [x] Change modes +- [x] Change configuration for robot +- [ ] SLAM (working in progress) +- [ ] Nav2 (working in progress) +- [ ] Ros2cli +- [ ] Hardware interface +- [ ] Gazebo simulation + +## Installation +You need to have previously installed ROS2. Please follow this [guide](https://docs.ros.org/en/humble/Installation.html) if you don't have it. + +```bash +source /opt/ros/humble/setup.bash +``` + +Create workspace and clone the repository + +```bash +mkdir ~/go2_ws/src +cd ~/go2_ws/src +git clone https://github.com/IntelligentRoboticsLabs/go2_robot.git -b humble +``` + +Install dependencies and build workspace +```bash +cd ~/go2_ws +sudo rosdep init +rosdep update +rosdep install --from-paths src --ignore-src -r -y +colcon build --symlink-install +``` + +Setup the workspace +```bash +source ~/ros2_ws/install/setup.bash +``` + +## Sensor installation +If you have purchased a hesai lidar 3d, or a realsense d435i, follow the following steps inside the robot. + +### Hesai Lidar + +```bash +sudo apt-get install libboost-all-dev +sudo apt-get install -y libyaml-cpp-dev +git clone --recurse-submodules https://github.com/HesaiTechnology/HesaiLidar_ROS_2.0.git +cd .. +source /opt/ros/$ROS_DISTRO/setup.bash +rosdep install --from-paths src --ignore-src -r -y +colcon build --symlink-install +source install/setup.bash +``` + +Set the lidar IP to `config/config.yaml` +```yaml +lidar: +- driver: + udp_port: 2368 #UDP port of lidar + ptc_port: 9347 #PTC port of lidar + device_ip_address: #IP address of lidar + pcap_path: "" #The path of pcap file (set during offline playback) + correction_file_path: "" #LiDAR angle file, required for offline playback of pcap/packet rosbag + firetimes_path: "" #The path of firetimes file + source_type: 2 #The type of data source, 1: real-time lidar connection, 2: pcap, 3: packet rosbag + pcap_play_synchronization: true #Pcap play rate synchronize with the host time + x: 0 #Calibration parameter + y: 0 #Calibration parameter + z: 0 #Calibration parameter + roll: 0 #Calibration parameter + pitch: 0 #Calibration parameter + yaw: 0 #Calibration parameter +ros: + ros_frame_id: hesai_lidar #Frame id of packet message and point cloud message + ros_recv_packet_topic: /lidar_packets #Topic used to receive lidar packets from ROS + ros_send_packet_topic: /lidar_packets #Topic used to send lidar packets through ROS + ros_send_point_cloud_topic: /lidar_points #Topic used to send point cloud through ROS + send_packet_ros: true #true: Send packets through ROS + send_point_cloud_ros: true #true: Send point cloud through ROS +``` + +### Realsense d435i + +```bash +sudo apt install ros-humble-realsense2-camera +``` + +## Usage +### Bringup the robot +Either from your computer, or from inside the robot, execute the following: +```bash +ros2 launch go2_bringup go2.launch.py +``` +> If you have a realsense and a lidar inside the robot, use the lidar or realsense parameters. +> It will only work if you throw everything inside the robot + +If you want to see your robot through rviz, do it as follows: +```bash +ros2 launch go2_bringup go2.launch.py rviz:=True +``` + +### Change modes +If what you want is for your robot to be able to change modes, thus performing the movements predefined by the controller, use the following service: +```bash +ros2 service call /mode go2_interfaces/srv/Mode "mode: 'hello'" +``` + +
+Available modes +``` +damp +balance_stand +stop_move +stand_up +stand_down +sit +rise_sit +hello +stretch +wallow +scrape +front_flip +front_jump +front_pounce +dance1 +dance2 +finger_heart +``` +
+ +### Change configurations for robot +If you want, you can modify the ways the robot walks, the height of the base, the height of the legs when walking... I show you the different parameters that can be modified: + +- BodyHeight: Set the relative height of the body above the ground when standing and walking. [0.3 ~ 0.5] + ```bash + ros2 service call /body_height go2_interfaces/srv/BodyHeight "height: 0.0" + ``` +- ContinuousGait: Continuous movement + ```bash + ros2 service call /continuous_gait go2_interfaces/srv/ContinuousGait "flag: false" + ``` +- Euler: Posture when standing and walking. [-0.75 ~ 0.75] [-0.75 ~ 0.75] [-1.5 ~ 1.5] + ```bash + ros2 service call /euler go2_interfaces/srv/Euler "roll: 0.0 pitch: 0.0 yaw: 0.0" + ``` +- FootRaiseHeight: Set the relative height of foot lift during movement [-0.06 ~ 0.03] + ```bash + ros2 service call /foot_raise_height go2_interfaces/srv/FootRaiseHeight "height: 0.0" + ``` +- Pose: Set true to pose and false to restore + ```bash + ros2 service call /pose go2_interfaces/srv/Pose "flag: false" + ``` +- SpeedLevel: Set the speed range [-1 ~ 1] + ```bash + ros2 service call /speed_level go2_interfaces/srv/SpeedLevel "level: 0" + ``` +- SwitchGait: Switch gait [0 - 4] + ```bash + ros2 service call /switch_gait go2_interfaces/srv/SwitchGait "d: 0" + ``` +- SwitchJoystick: Native remote control response switch + ```bash + ros2 service call /switch_joystick go2_interfaces/srv/SwitchJoystick "flag: false" + ``` + +## SLAM +In the future, work in progress. + +## NAVIGATION +In the future, work in progress. + +## Demos +### Robot description + +[go2_tf.webm](https://github.com/IntelligentRoboticsLabs/go2_robot/assets/44479765/b019e6fc-875f-4870-a510-afa6e1353e08) + +### Robot PointCloud + +https://github.com/IntelligentRoboticsLabs/go2_robot/assets/44479765/c164840a-6857-4d95-a50e-023c5bb44edb + +## Acknowledgment +Thanks to [unitree](https://github.com/unitreerobotics/unitree_ros2) for providing the support and communication interfaces with the robot. + +## About +This is a project made by the [Intelligent Robotics Lab](https://intelligentroboticslab.gsyc.urjc.es/), a research group from the [Universidad Rey Juan Carlos](https://www.urjc.es/). +Copyright © 2024. + +Maintainers: + +* [Juan Carlos Manzanares Serrano](https://github.com/Juancams) +* [Juan S. Cely](https://github.com/juanscelyg) + +## License + +This project is licensed under the BSD 3-clause License - see the [LICENSE](https://github.com/IntelligentRoboticsLabs/go2_robot/blob/humble/LICENSE) file for details. diff --git a/go2_bringup/CMakeLists.txt b/go2_bringup/CMakeLists.txt new file mode 100644 index 0000000..d4303ee --- /dev/null +++ b/go2_bringup/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) +project(go2_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/go2_bringup/config/empty b/go2_bringup/config/empty new file mode 100644 index 0000000..e69de29 diff --git a/go2_bringup/launch/go2.launch.py b/go2_bringup/launch/go2.launch.py new file mode 100644 index 0000000..ab243a0 --- /dev/null +++ b/go2_bringup/launch/go2.launch.py @@ -0,0 +1,107 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression + + +def generate_launch_description(): + lidar = LaunchConfiguration('lidar') + realsense = LaunchConfiguration('realsense') + rviz = LaunchConfiguration('rviz') + + declare_lidar_cmd = DeclareLaunchArgument( + 'lidar', + default_value='False', + description='Launch hesai lidar driver' + ) + + declare_realsense_cmd = DeclareLaunchArgument( + 'realsense', + default_value='False', + description='Launch realsense driver' + ) + + declare_rviz_cmd = DeclareLaunchArgument( + 'rviz', + default_value='False', + description='Launch rviz' + ) + + robot_description_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('go2_description'), + 'launch/'), 'robot.launch.py']) + ) + + driver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('go2_driver'), + 'launch/'), 'go2_driver.launch.py']) + ) + + lidar_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('hesai_ros_driver'), + 'launch/'), 'start.py']), + condition=IfCondition(PythonExpression([lidar])) + ) + + realsense_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), + 'launch/'), 'rs_launch.py']), + condition=IfCondition(PythonExpression([realsense])) + ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('go2_rviz'), + 'launch/'), 'rviz.launch.py']), + condition=IfCondition(PythonExpression([rviz])) + ) + + ld = LaunchDescription() + ld.add_action(declare_lidar_cmd) + ld.add_action(declare_realsense_cmd) + ld.add_action(declare_rviz_cmd) + ld.add_action(robot_description_cmd) + ld.add_action(lidar_cmd) + ld.add_action(realsense_cmd) + ld.add_action(driver_cmd) + ld.add_action(rviz_cmd) + + return ld diff --git a/go2_bringup/package.xml b/go2_bringup/package.xml new file mode 100644 index 0000000..64ee390 --- /dev/null +++ b/go2_bringup/package.xml @@ -0,0 +1,22 @@ + + + + go2_bringup + 1.0.0 + TODO: Package description + Juan Carlos Manzanares Serrano + BSD 3-Clause License + + ament_cmake + + go2_description + go2_driver + go2_rviz + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/go2_description/launch/robot.launch.py b/go2_description/launch/robot.launch.py index 51991fc..89e2f47 100644 --- a/go2_description/launch/robot.launch.py +++ b/go2_description/launch/robot.launch.py @@ -32,79 +32,73 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare import launch_ros.descriptions +from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - declared_arguments = [] - nodes = [] + declared_arguments = [] + nodes = [] - declared_arguments.append( + declared_arguments.append( + DeclareLaunchArgument( + 'description_package', + default_value='go2_description', + description='Description package with robot URDF/xacro files made by manufacturer.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + 'description_file', + default_value='go2_description.urdf', + description='URDF/XACRO description file with the robot.', + ) + ) + declared_arguments.append( DeclareLaunchArgument( - "description_package", - default_value="go2_description", - description="Description package with robot URDF/xacro files made by manufacturer.", + 'prefix', + default_value='', + description='Prefix to be added to the robot description.', ) ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="go2_description.urdf", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value="", - description="Prefix to be added to the robot description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_sim_time", - default_value="True", - description="Use simulation/Gazebo clock if true", - ) - ) - - description_file = LaunchConfiguration("description_file") - prefix = LaunchConfiguration("prefix") - use_sim_time = LaunchConfiguration('use_sim_time') - - robot_description_content = Command( + declared_arguments.append( + DeclareLaunchArgument( + 'use_sim_time', + default_value='True', + description='Use simulation/Gazebo clock if true', + ) + ) + + description_file = LaunchConfiguration('description_file') + prefix = LaunchConfiguration('prefix') + use_sim_time = LaunchConfiguration('use_sim_time') + + robot_description_content = Command( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution([FindPackageShare("go2_description"), "urdf", description_file]), - " ", - "prefix:=", prefix + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([FindPackageShare('go2_description'), 'urdf', description_file]), + ' ', + 'prefix:=', prefix ] ) - - robot_description_param = launch_ros.descriptions.ParameterValue(robot_description_content, value_type=str) - nodes.append(Node( + robot_description_param = launch_ros.descriptions.ParameterValue(robot_description_content, + value_type=str) + + nodes.append(Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', parameters=[{ - 'use_sim_time': use_sim_time, - 'robot_description': robot_description_param, - 'publish_frequency': 100.0, - 'frame_prefix': '', - }], + 'use_sim_time': use_sim_time, + 'robot_description': robot_description_param, + 'publish_frequency': 100.0, + 'frame_prefix': '', + }], + ) ) - ) - nodes.append(Node( - package='rviz2', - namespace='', - executable='rviz2', - name='rviz2', - ) - ) - - return LaunchDescription(declared_arguments + nodes) + return LaunchDescription(declared_arguments + nodes) diff --git a/go2_description/package.xml b/go2_description/package.xml index ec4616c..7f72d06 100644 --- a/go2_description/package.xml +++ b/go2_description/package.xml @@ -2,7 +2,7 @@ go2_description - 0.0.1 + 1.0.0 Unitree go2 robot description Juan Carlos Manzanares Serrano BSD 3-Clause License diff --git a/go2_description/urdf/go2_description.urdf b/go2_description/urdf/go2_description.urdf index 9ada169..eeac215 100644 --- a/go2_description/urdf/go2_description.urdf +++ b/go2_description/urdf/go2_description.urdf @@ -6,7 +6,7 @@ Stephen Brawner (brawner@gmail.com) + name="base_link"> + link="base_link" /> + link="base_link" /> + link="base_link" /> + link="base_link" /> + link="base_link" /> + link="base_link" /> + link="base_link" /> +#include +#include +#include +#include +#include +#include +#include +#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 +{ + +class Go2Driver : public rclcpp::Node +{ +public: + Go2Driver(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + void publish_lidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + void publish_pose_stamped(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg); + 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 request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleContinuousGait( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleEuler( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleFootRaiseHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleMode( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handlePose( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSpeedLevel( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSwitchGait( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + void handleSwitchJoystick( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response); + + rclcpp::Subscription::SharedPtr pointcloud_sub_; + rclcpp::Subscription::SharedPtr robot_pose_sub_; + rclcpp::Subscription::SharedPtr joy_sub_; + rclcpp::Subscription::SharedPtr low_state_sub_; + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + + rclcpp::Publisher::SharedPtr pointcloud_pub_; + rclcpp::Publisher::SharedPtr joint_state_pub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr request_pub_; + + rclcpp::Service::SharedPtr set_body_height_service_; + rclcpp::Service::SharedPtr set_continuous_gait_service_; + rclcpp::Service::SharedPtr set_euler_service_; + rclcpp::Service::SharedPtr set_foot_raise_height_service_; + rclcpp::Service::SharedPtr set_mode_service_; + rclcpp::Service::SharedPtr set_pose_service_; + rclcpp::Service::SharedPtr set_speed_level_service_; + rclcpp::Service::SharedPtr set_switch_gait_service_; + rclcpp::Service::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 + +#endif // GO2_DRIVER__GO2_DRIVER_HPP_ diff --git a/go2_driver/launch/go2_driver.launch.py b/go2_driver/launch/go2_driver.launch.py new file mode 100644 index 0000000..fc0541e --- /dev/null +++ b/go2_driver/launch/go2_driver.launch.py @@ -0,0 +1,75 @@ +# 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. + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + + composable_nodes = [] + + composable_node = ComposableNode( + package='go2_driver', + plugin='go2_driver::Go2Driver', + name='go2_driver', + namespace='', + + ) + composable_nodes.append(composable_node) + + container = ComposableNodeContainer( + name='go2_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=composable_nodes, + output='screen', + ) + + pointclod_to_laserscan_cmd = Node( + package='pointcloud_to_laserscan', + executable='pointcloud_to_laserscan_node', + name='pointcloud_to_laserscan', + namespace='', + output='screen', + remappings=[('/cloud_in', '/pointcloud')], + parameters=[{ + 'target_frame': 'radar', + 'transform_tolerance': 0.01, + }], + ) + + ld = LaunchDescription() + ld.add_action(container) + ld.add_action(pointclod_to_laserscan_cmd) + + return ld diff --git a/go2_driver/package.xml b/go2_driver/package.xml new file mode 100644 index 0000000..480d135 --- /dev/null +++ b/go2_driver/package.xml @@ -0,0 +1,29 @@ + + + + go2_driver + 1.0.0 + GO2 robot driver package + Juan Carlos Manzanares Serrano + BSD 3-Clause License + + ament_cmake + + rclcpp + rclcpp_lifecycle + rclcpp_components + sensor_msgs + geometry_msgs + nav_msgs + unitree_go + unitree_api + tf2_ros + go2_interfaces + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/go2_driver/src/go2_driver/go2_driver.cpp b/go2_driver/src/go2_driver/go2_driver.cpp new file mode 100644 index 0000000..beb5804 --- /dev/null +++ b/go2_driver/src/go2_driver/go2_driver.cpp @@ -0,0 +1,465 @@ +// 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. + +#include + +namespace go2_driver +{ + +Go2Driver::Go2Driver( + const rclcpp::NodeOptions & options) +: Node("go2_driver", options), + tf_broadcaster_(this) +{ + rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + qos_profile.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + + pointcloud_pub_ = create_publisher("pointcloud", 10); + joint_state_pub_ = create_publisher("joint_states", 10); + odom_pub_ = create_publisher("odom", qos_profile); + imu_pub_ = create_publisher("imu", 10); + request_pub_ = create_publisher("api/sport/request", 10); + + pointcloud_sub_ = create_subscription( + "/utlidar/cloud", 10, + std::bind(&Go2Driver::publish_lidar, this, std::placeholders::_1)); + + robot_pose_sub_ = create_subscription( + "/utlidar/robot_pose", 10, + std::bind(&Go2Driver::publish_pose_stamped, this, std::placeholders::_1)); + + joy_sub_ = create_subscription( + "joy", 10, std::bind(&Go2Driver::joy_callback, this, std::placeholders::_1)); + + low_state_sub_ = create_subscription( + "lowstate", 10, + std::bind(&Go2Driver::publish_joint_states, this, std::placeholders::_1)); + + cmd_vel_sub_ = create_subscription( + "cmd_vel", 10, std::bind(&Go2Driver::cmd_vel_callback, this, std::placeholders::_1)); + + set_body_height_service_ = + this->create_service( + "body_height", + std::bind( + &Go2Driver::handleBodyHeight, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_continuous_gait_service_ = + this->create_service( + "continuous_gait", + std::bind( + &Go2Driver::handleContinuousGait, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_euler_service_ = + this->create_service( + "euler", + std::bind( + &Go2Driver::handleEuler, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_foot_raise_height_service_ = + this->create_service( + "foot_raise_height", + std::bind( + &Go2Driver::handleFootRaiseHeight, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_mode_service_ = + this->create_service( + "mode", + std::bind( + &Go2Driver::handleMode, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_pose_service_ = + this->create_service( + "pose", + std::bind( + &Go2Driver::handlePose, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_speed_level_service_ = + this->create_service( + "speed_level", + std::bind( + &Go2Driver::handleSpeedLevel, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_switch_gait_service_ = + this->create_service( + "switch_gait", + std::bind( + &Go2Driver::handleSwitchGait, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + set_switch_joystick_service_ = + this->create_service( + "switch_joystick", + std::bind( + &Go2Driver::handleSwitchJoystick, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); +} + +void Go2Driver::publish_lidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg) +{ + msg->header.stamp = now(); + msg->header.frame_id = "radar"; + pointcloud_pub_->publish(*msg); +} + +void Go2Driver::publish_pose_stamped(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +{ + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = now(); + transform.header.frame_id = "odom"; + transform.child_frame_id = "base_link"; + transform.transform.translation.x = msg->pose.position.x; + transform.transform.translation.y = msg->pose.position.y; + transform.transform.translation.z = msg->pose.position.z + 0.07; + transform.transform.rotation.x = msg->pose.orientation.x; + transform.transform.rotation.y = msg->pose.orientation.y; + transform.transform.rotation.z = msg->pose.orientation.z; + transform.transform.rotation.w = msg->pose.orientation.w; + tf_broadcaster_.sendTransform(transform); + + if (!odom_published_) { + nav_msgs::msg::Odometry odom; + odom.header.stamp = now(); + odom.header.frame_id = "odom"; + odom.child_frame_id = "base_link"; + odom.pose.pose.position.x = msg->pose.position.x; + odom.pose.pose.position.y = msg->pose.position.y; + odom.pose.pose.position.z = msg->pose.position.z + 0.07; + odom.pose.pose.orientation.x = msg->pose.orientation.x; + odom.pose.pose.orientation.y = msg->pose.orientation.y; + odom.pose.pose.orientation.z = msg->pose.orientation.z; + odom.pose.pose.orientation.w = msg->pose.orientation.w; + odom_pub_->publish(odom); + odom_published_ = true; + } +} + +void Go2Driver::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) +{ + joy_state_ = *msg; +} + +void Go2Driver::publish_joint_states(const unitree_go::msg::LowState::SharedPtr msg) +{ + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = now(); + joint_state.name = {"FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", + "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", + "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint", + "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint"}; + + joint_state.position = {msg->motor_state[3].q, msg->motor_state[4].q, msg->motor_state[5].q, + msg->motor_state[0].q, msg->motor_state[1].q, msg->motor_state[2].q, + msg->motor_state[9].q, msg->motor_state[10].q, msg->motor_state[11].q, + msg->motor_state[6].q, msg->motor_state[7].q, msg->motor_state[8].q}; + + joint_state_pub_->publish(joint_state); +} + +void Go2Driver::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + nlohmann::json js; + js["x"] = msg->linear.x; + js["y"] = msg->linear.y; + js["z"] = msg->angular.z; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::Move); + + request_pub_->publish(req); +} + +void Go2Driver::handleBodyHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->height < -0.18 || request->height > 0.03) { + response->success = false; + response->message = "Height value is out of range [0.3 ~ 0.5]"; + return; + } + + nlohmann::json js; + js["data"] = request->height; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::BodyHeight); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleContinuousGait( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + js["data"] = request->flag; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::ContinuousGait); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleEuler( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + if (request->roll < -0.75 || request->roll > 0.75) { + response->success = false; + response->message = "Roll value is out of range [-0.75 ~ 0.75]"; + return; + } else if (request->pitch < -0.75 || request->pitch > 0.75) { + response->success = false; + response->message = "Pitch value is out of range [-0.75 ~ 0.75]"; + return; + } else if (request->yaw < -0.6 || request->yaw > 0.6) { + response->success = false; + response->message = "Yaw value is out of range [-1.5 ~ 1.5]"; + return; + } + + js["x"] = request->roll; + js["y"] = request->pitch; + js["z"] = request->yaw; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::Euler); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleFootRaiseHeight( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->height < 0 || request->height > 0.1) { + response->success = false; + response->message = "Height value is out of range [-0.06 ~ 0.03]"; + return; + } + + nlohmann::json js; + js["data"] = request->height; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::FootRaiseHeight); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleMode( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + std::string mode = request->mode; + + unitree_api::msg::Request req; + + if (mode == "damp") { + response->message = "Change the mode to Damp"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Damp); + } else if (mode == "balance_stand") { + response->message = "Change the mode to BalanceStand"; + req.header.identity.api_id = static_cast(go2_driver::Mode::BalanceStand); + } else if (mode == "stop_move") { + response->message = "Change the mode to StopMove"; + req.header.identity.api_id = static_cast(go2_driver::Mode::StopMove); + } else if (mode == "stand_up") { + response->message = "Change the mode to StandUp"; + req.header.identity.api_id = static_cast(go2_driver::Mode::StandUp); + } else if (mode == "stand_down") { + response->message = "Change the mode to StandDown"; + req.header.identity.api_id = static_cast(go2_driver::Mode::StandDown); + } else if (mode == "sit") { + response->message = "Change the mode to Sit"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Sit); + } else if (mode == "rise_sit") { + response->message = "Change the mode to RiseSit"; + req.header.identity.api_id = static_cast(go2_driver::Mode::RiseSit); + } else if (mode == "hello") { + response->message = "Change the mode to Hello. Say hello to your robot!"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Hello); + } else if (mode == "stretch") { + response->message = "Change the mode to Stretch"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Stretch); + } else if (mode == "wallow") { + response->message = "Change the mode to Wallow"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Wallow); + } else if (mode == "scrape") { + response->message = "Change the mode to Scrape"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Scrape); + } else if (mode == "front_flip") { + response->message = "Front flip??? Really? You want to break your robot? Crazy!"; + // req.header.identity.api_id = static_cast(go2_driver::Mode::FrontFlip); + } else if (mode == "front_jump") { + response->message = "Change the mode to Front Jump"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FrontJump); + } else if (mode == "front_pounce") { + response->message = "Change the mode to Front Pounce"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FrontPounce); + } else if (mode == "dance1") { + response->message = "Change the mode to Dance 1. Let's dance!"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Dance1); + } else if (mode == "dance2") { + response->message = "Change the mode to Dance 2. Let's dance!"; + req.header.identity.api_id = static_cast(go2_driver::Mode::Dance2); + } else if (mode == "finger_heart") { + response->message = "Change the mode to Finger Heart"; + req.header.identity.api_id = static_cast(go2_driver::Mode::FingerHeart); + } else { + response->success = false; + response->message = "Invalid mode"; + return; + } + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handlePose( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + js["data"] = request->flag; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::Pose); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleSpeedLevel( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->level < -1 || request->level > 1) { + response->success = false; + response->message = "Speed level is out of range [-1 ~ 1]"; + return; + } + + nlohmann::json js; + js["data"] = request->level; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::SpeedLevel); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleSwitchGait( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + if (request->d < 0 || request->d > 4) { + response->success = false; + response->message = "Invalid gait type [0 - 4]"; + return; + } + + nlohmann::json js; + js["data"] = request->d; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::SwitchGait); + + request_pub_->publish(req); + response->success = true; +} + +void Go2Driver::handleSwitchJoystick( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) +{ + (void)request_header; + + nlohmann::json js; + js["data"] = request->flag; + + unitree_api::msg::Request req; + req.parameter = js.dump(); + req.header.identity.api_id = static_cast(go2_driver::Mode::SwitchJoystick); + + request_pub_->publish(req); + response->success = true; +} + +} // namespace go2_driver + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(go2_driver::Go2Driver) diff --git a/go2_hardware/CMakeLists.txt b/go2_hardware/CMakeLists.txt new file mode 100644 index 0000000..63c9d38 --- /dev/null +++ b/go2_hardware/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.8) +project(go2_hardware) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/go2_hardware/include/empty b/go2_hardware/include/empty new file mode 100644 index 0000000..e69de29 diff --git a/go2_hardware/package.xml b/go2_hardware/package.xml new file mode 100644 index 0000000..fe6dc5c --- /dev/null +++ b/go2_hardware/package.xml @@ -0,0 +1,18 @@ + + + + go2_hardware + 0.0.0 + TODO: Package description + Juan Carlos Manzanares Serrano + BSD 3-Clause License + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/go2_hardware/src/empty b/go2_hardware/src/empty new file mode 100644 index 0000000..e69de29 diff --git a/go2_interfaces/CMakeLists.txt b/go2_interfaces/CMakeLists.txt index 7e22eb1..1a22ec5 100755 --- a/go2_interfaces/CMakeLists.txt +++ b/go2_interfaces/CMakeLists.txt @@ -1,79 +1,33 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(go2_interfaces) -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) -find_package(rosidl_generator_dds_idl REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - "msg/AudioData.msg" - "msg/BmsCmd.msg" - "msg/BmsState.msg" - "msg/Error.msg" - "msg/Go2FrontVideoData.msg" - "msg/HeightMap.msg" - "msg/IMUState.msg" - "msg/InterfaceConfig.msg" - "msg/LidarState.msg" - "msg/LowCmd.msg" - "msg/LowState.msg" - "msg/MotorCmd.msg" - "msg/MotorCmds.msg" - "msg/MotorState.msg" - "msg/MotorStates.msg" - "msg/PathPoint.msg" - "msg/Req.msg" - "msg/Res.msg" - "msg/SportModeCmd.msg" - "msg/SportModeState.msg" - "msg/TimeSpec.msg" - "msg/UwbState.msg" - "msg/UwbSwitch.msg" - "msg/WirelessController.msg" - DEPENDENCIES geometry_msgs -) - -rosidl_generate_dds_interfaces( - ${rosidl_generate_interfaces_TARGET}__dds_connext_idl - IDL_TUPLES ${rosidl_generate_interfaces_IDL_TUPLES} - OUTPUT_SUBFOLDERS "dds_connext" + "srv/BodyHeight.srv" + "srv/ContinuousGait.srv" + "srv/Euler.srv" + "srv/FootRaiseHeight.srv" + "srv/Mode.srv" + "srv/Pose.srv" + "srv/SpeedLevel.srv" + "srv/SwitchGait.srv" + "srv/SwitchJoystick.srv" + DEPENDENCIES ) -add_dependencies( - ${PROJECT_NAME} - ${PROJECT_NAME}__dds_connext_idl -) - - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() -ament_package() +ament_package() \ No newline at end of file diff --git a/go2_interfaces/msg/Go2FrontVideoData.msg b/go2_interfaces/msg/Go2FrontVideoData.msg deleted file mode 100755 index 3c294f3..0000000 --- a/go2_interfaces/msg/Go2FrontVideoData.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint64 time_frame -uint8[] video720p -uint8[] video360p -uint8[] video180p \ No newline at end of file diff --git a/go2_interfaces/package.xml b/go2_interfaces/package.xml index 8c7bbcc..f0fd5c8 100755 --- a/go2_interfaces/package.xml +++ b/go2_interfaces/package.xml @@ -2,17 +2,17 @@ go2_interfaces - 0.0.1 - Communication interfaces of the unitree go 2 robots - Unitree + 1.0.0 + Go2 Interfaces + Juan Carlos Manzanares BSD 3-Clause License - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages ament_cmake + rosidl_default_generators + + rosidl_default_generators - geometry_msgs + rosidl_interface_packages ament_lint_auto ament_lint_common diff --git a/go2_interfaces/srv/BodyHeight.srv b/go2_interfaces/srv/BodyHeight.srv new file mode 100644 index 0000000..4cc898d --- /dev/null +++ b/go2_interfaces/srv/BodyHeight.srv @@ -0,0 +1,6 @@ +# Set the relative height of the body above the ground when standing and walking. +float32 height + +--- +bool success +string message \ No newline at end of file diff --git a/go2_interfaces/srv/ContinuousGait.srv b/go2_interfaces/srv/ContinuousGait.srv new file mode 100644 index 0000000..44f3607 --- /dev/null +++ b/go2_interfaces/srv/ContinuousGait.srv @@ -0,0 +1,6 @@ +# Continuous movement + +bool flag + +--- +bool success \ No newline at end of file diff --git a/go2_interfaces/srv/Euler.srv b/go2_interfaces/srv/Euler.srv new file mode 100644 index 0000000..f46af61 --- /dev/null +++ b/go2_interfaces/srv/Euler.srv @@ -0,0 +1,9 @@ +# Posture when standing and walking + +float32 roll +float32 pitch +float32 yaw + +--- +bool success +string message \ No newline at end of file diff --git a/go2_interfaces/srv/FootRaiseHeight.srv b/go2_interfaces/srv/FootRaiseHeight.srv new file mode 100644 index 0000000..3f59531 --- /dev/null +++ b/go2_interfaces/srv/FootRaiseHeight.srv @@ -0,0 +1,7 @@ +# Set the relative height of foot lift during movement + +float32 height + +--- +bool success +string message \ No newline at end of file diff --git a/go2_interfaces/srv/Mode.srv b/go2_interfaces/srv/Mode.srv new file mode 100644 index 0000000..9e540fd --- /dev/null +++ b/go2_interfaces/srv/Mode.srv @@ -0,0 +1,5 @@ +string mode + +--- +bool success +string message \ No newline at end of file diff --git a/go2_interfaces/srv/Pose.srv b/go2_interfaces/srv/Pose.srv new file mode 100644 index 0000000..1afd315 --- /dev/null +++ b/go2_interfaces/srv/Pose.srv @@ -0,0 +1,6 @@ +# Set true to pose and false to restore + +bool flag + +--- +bool success \ No newline at end of file diff --git a/go2_interfaces/srv/SpeedLevel.srv b/go2_interfaces/srv/SpeedLevel.srv new file mode 100644 index 0000000..6d240c3 --- /dev/null +++ b/go2_interfaces/srv/SpeedLevel.srv @@ -0,0 +1,7 @@ +# Set the speed range + +int32 level + +--- +bool success +string message \ No newline at end of file diff --git a/go2_interfaces/srv/SwitchGait.srv b/go2_interfaces/srv/SwitchGait.srv new file mode 100644 index 0000000..eef6b6e --- /dev/null +++ b/go2_interfaces/srv/SwitchGait.srv @@ -0,0 +1,13 @@ +# Switch gait + +int32 IDLE = 0 +int32 TROT = 1 +int32 TROT_RUN_NING = 2 +int32 FORWARD_CLIMBING_MODE = 3 +int32 REVERSE_CLIMBING_MODE = 4 + +int32 d + +--- +bool success +string message \ No newline at end of file diff --git a/go2_interfaces/srv/SwitchJoystick.srv b/go2_interfaces/srv/SwitchJoystick.srv new file mode 100644 index 0000000..329f4d6 --- /dev/null +++ b/go2_interfaces/srv/SwitchJoystick.srv @@ -0,0 +1,6 @@ +# Native remote control response switch + +bool flag + +--- +bool success \ No newline at end of file diff --git a/go2_nav/CMakeLists.txt b/go2_nav/CMakeLists.txt new file mode 100644 index 0000000..a9a26f4 --- /dev/null +++ b/go2_nav/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) +project(go2_nav) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY launch params + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/go2_nav/launch/navigation.launch.py b/go2_nav/launch/navigation.launch.py new file mode 100644 index 0000000..15a4c07 --- /dev/null +++ b/go2_nav/launch/navigation.launch.py @@ -0,0 +1,127 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import SetRemap + + +def generate_launch_description(): + package_dir = get_package_share_directory('go2_nav') + nav2_dir = get_package_share_directory('nav2_bringup') + + # Configuration Variables + use_sim_time = LaunchConfiguration('use_sim_time') + slam = LaunchConfiguration('slam') + rviz = LaunchConfiguration('rviz') + map_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + namespace = LaunchConfiguration('namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', default_value='false') + + declare_slam_cmd = DeclareLaunchArgument( + 'slam', default_value='False') + + declare_use_rviz_cmd = DeclareLaunchArgument( + 'rviz', default_value='True') + + declare_map_cmd = DeclareLaunchArgument( + 'map', default_value='') + + declare_nav_params_cmd = DeclareLaunchArgument( + 'params_file', default_value=os.path.join( + package_dir, + 'params', + 'go2_nav_params.yaml') + ) + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='' + ) + + # Actions + localization_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_dir, 'launch', 'localization_launch.py') + ), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'slam': slam, + 'map': map_file, + 'params_file': params_file, + 'namespace': namespace + }.items() + ) + + navigation_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_dir, 'launch', 'navigation_launch.py') + ), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'namespace': namespace + }.items() + ) + + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_dir, 'launch', 'rviz_launch.py') + ), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'rviz': rviz, + 'namespace': namespace + }.items() + ) + + # Remappings + cmd_vel_remap = SetRemap(src='cmd_vel_nav', dst='cmd_vel') + + ld = LaunchDescription() + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_nav_params_cmd) + ld.add_action(declare_use_rviz_cmd) + ld.add_action(declare_map_cmd) + ld.add_action(declare_namespace_cmd) + ld.add_action(localization_cmd) + ld.add_action(navigation_cmd) + ld.add_action(rviz_cmd) + ld.add_action(cmd_vel_remap) + + return ld diff --git a/go2_nav/launch/slam.launch.py b/go2_nav/launch/slam.launch.py new file mode 100644 index 0000000..e69de29 diff --git a/go2_nav/package.xml b/go2_nav/package.xml new file mode 100644 index 0000000..76e0bae --- /dev/null +++ b/go2_nav/package.xml @@ -0,0 +1,18 @@ + + + + go2_nav + 0.0.1 + Unitree go2 robot navigation + Juan Carlos Manzanares Serrano + BSD 3-Clause License + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/go2_nav/params/go2_nav_params.yaml b/go2_nav/params/go2_nav_params.yaml new file mode 100644 index 0000000..9de5dd5 --- /dev/null +++ b/go2_nav/params/go2_nav_params.yaml @@ -0,0 +1,358 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::OmniMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: /scan + set_initial_pose: true + initial_pose: + x: 0.0 + y: 0.0 + z: 0.0 + yaw: 0.0 + + +amcl_map_client: + ros__parameters: + use_sim_time: False + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: False + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.30 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.30 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.4 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 1.5 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.4 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 1.5 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 \ No newline at end of file diff --git a/go2_robot/package.xml b/go2_robot/package.xml index 75845ab..14aa5c2 100644 --- a/go2_robot/package.xml +++ b/go2_robot/package.xml @@ -1,7 +1,7 @@ go2_robot - 0.0.1 + 1.0.0 The unitree go2 robot package Juan Carlos Manzanares Serrano BSD 3-Clause License diff --git a/go2_rviz/CMakeLists.txt b/go2_rviz/CMakeLists.txt new file mode 100644 index 0000000..69e6adb --- /dev/null +++ b/go2_rviz/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) +project(go2_rviz) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/go2_rviz/config/go2_rviz.rviz b/go2_rviz/config/go2_rviz.rviz new file mode 100644 index 0000000..cdf3c7e --- /dev/null +++ b/go2_rviz/config/go2_rviz.rviz @@ -0,0 +1,471 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 89 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 443 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + FL_calf: + Value: true + FL_calflower: + Value: true + FL_calflower1: + Value: true + FL_foot: + Value: true + FL_hip: + Value: true + FL_thigh: + Value: true + FR_calf: + Value: true + FR_calflower: + Value: true + FR_calflower1: + Value: true + FR_foot: + Value: true + FR_hip: + Value: true + FR_thigh: + Value: true + Head_lower: + Value: true + Head_upper: + Value: true + RL_calf: + Value: true + RL_calflower: + Value: true + RL_calflower1: + Value: true + RL_foot: + Value: true + RL_hip: + Value: true + RL_thigh: + Value: true + RR_calf: + Value: true + RR_calflower: + Value: true + RR_calflower1: + Value: true + RR_foot: + Value: true + RR_hip: + Value: true + RR_thigh: + Value: true + base_link: + Value: true + camera_link: + Value: true + hesai_lidar: + Value: true + imu: + Value: true + odom: + Value: true + radar: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + odom: + base_link: + FL_hip: + FL_thigh: + FL_calf: + FL_calflower: + FL_calflower1: + {} + FL_foot: + {} + FR_hip: + FR_thigh: + FR_calf: + FR_calflower: + FR_calflower1: + {} + FR_foot: + {} + Head_upper: + Head_lower: + {} + camera_link: + {} + RL_hip: + RL_thigh: + RL_calf: + RL_calflower: + RL_calflower1: + {} + RL_foot: + {} + RR_hip: + RR_thigh: + RR_calf: + RR_calflower: + RR_calflower1: + {} + RR_foot: + {} + hesai_lidar: + {} + imu: + {} + radar: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + FL_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_calflower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_calflower1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_calflower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_calflower1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Head_lower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Head_upper: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RL_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_calflower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_calflower1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_calflower: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_calflower1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hesai_lidar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu: + Alpha: 1 + Show Axes: false + Show Trail: false + radar: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 216 + Min Color: 0; 0; 0 + Min Intensity: 74 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.064781665802002 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.02936464361846447 + Y: 0.031766824424266815 + Z: 0.3274531662464142 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4752010107040405 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.166723251342773 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 751 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000003000000000000015600000251fc020000000bfb0000001200530065006c0065006300740069006f006e000000003d000000820000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000251000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000480043006f0072006500730065006e0073006500200049006e0073007400720075006d0065006e0074006100740069006f006e0020005200760069007a002000500061006e0065006c01000002de000000b50000000000000000fb0000001e0043006f0072006500730065006e00730065005f00500061006e0065006c0100000176000001180000000000000000fb0000000a0049006d00610067006501000002e8000000ab0000000000000000000000010000015d0000037cfc0200000005fb0000000a00560069006500770073000000003d0000037c000000a400fffffffb0000003e0043006f0072006500730065006e0073006500200049006e0073007400720075006d0065006e0074006100740069006f006e002000500061006e0065006c000000003d0000037c0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000251000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000003000005a00000003efc0100000002fb0000000800540069006d00650100000000000005a0000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004440000025100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1440 + X: 2208 + Y: 74 diff --git a/go2_rviz/launch/rviz.launch.py b/go2_rviz/launch/rviz.launch.py new file mode 100644 index 0000000..369a4dc --- /dev/null +++ b/go2_rviz/launch/rviz.launch.py @@ -0,0 +1,50 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', + os.path.join(get_package_share_directory('go2_rviz'), + 'config', 'go2_rviz.rviz')] + ) + ]) diff --git a/go2_rviz/package.xml b/go2_rviz/package.xml new file mode 100644 index 0000000..f94028b --- /dev/null +++ b/go2_rviz/package.xml @@ -0,0 +1,18 @@ + + + + go2_rviz + 1.0 + Unitree go2 rviz + Juan Carlos Manzanares Serrano + BSD 3-Clause License + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/go2_simulation/CMakeLists.txt b/go2_simulation/CMakeLists.txt new file mode 100644 index 0000000..5b8b301 --- /dev/null +++ b/go2_simulation/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.8) +project(go2_simulation) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/go2_simulation/launch/empty b/go2_simulation/launch/empty new file mode 100644 index 0000000..e69de29 diff --git a/go2_simulation/package.xml b/go2_simulation/package.xml new file mode 100644 index 0000000..b8c584e --- /dev/null +++ b/go2_simulation/package.xml @@ -0,0 +1,18 @@ + + + + go2_simulation + 0.0.0 + TODO: Package description + Juan Carlos Manzanares Serrano + BSD 3-Clause License + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/go2_simulation/robots/empty b/go2_simulation/robots/empty new file mode 100644 index 0000000..e69de29 diff --git a/unitree_api/CMakeLists.txt b/unitree_api/CMakeLists.txt new file mode 100755 index 0000000..4e7c4b9 --- /dev/null +++ b/unitree_api/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.5) +project(unitree_api) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rosidl_generator_dds_idl REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Request.msg" + "msg/RequestHeader.msg" + "msg/RequestIdentity.msg" + "msg/RequestLease.msg" + "msg/RequestPolicy.msg" + "msg/Response.msg" + "msg/ResponseHeader.msg" + "msg/ResponseStatus.msg" + + DEPENDENCIES geometry_msgs +) + +rosidl_generate_dds_interfaces( + ${rosidl_generate_interfaces_TARGET}__dds_connext_idl + IDL_TUPLES ${rosidl_generate_interfaces_IDL_TUPLES} + OUTPUT_SUBFOLDERS "dds_connext" +) +add_dependencies( + ${PROJECT_NAME} + ${PROJECT_NAME}__dds_connext_idl +) + + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/unitree_api/msg/Request.msg b/unitree_api/msg/Request.msg new file mode 100755 index 0000000..b1df58a --- /dev/null +++ b/unitree_api/msg/Request.msg @@ -0,0 +1,3 @@ +RequestHeader header +string parameter +uint8[] binary \ No newline at end of file diff --git a/unitree_api/msg/RequestHeader.msg b/unitree_api/msg/RequestHeader.msg new file mode 100755 index 0000000..022161b --- /dev/null +++ b/unitree_api/msg/RequestHeader.msg @@ -0,0 +1,3 @@ +RequestIdentity identity +RequestLease lease +RequestPolicy policy \ No newline at end of file diff --git a/unitree_api/msg/RequestIdentity.msg b/unitree_api/msg/RequestIdentity.msg new file mode 100755 index 0000000..90011ff --- /dev/null +++ b/unitree_api/msg/RequestIdentity.msg @@ -0,0 +1,2 @@ +int64 id +int64 api_id \ No newline at end of file diff --git a/unitree_api/msg/RequestLease.msg b/unitree_api/msg/RequestLease.msg new file mode 100755 index 0000000..85f692f --- /dev/null +++ b/unitree_api/msg/RequestLease.msg @@ -0,0 +1 @@ +int64 id \ No newline at end of file diff --git a/unitree_api/msg/RequestPolicy.msg b/unitree_api/msg/RequestPolicy.msg new file mode 100755 index 0000000..89e00c2 --- /dev/null +++ b/unitree_api/msg/RequestPolicy.msg @@ -0,0 +1,2 @@ +int32 priority +bool noreply \ No newline at end of file diff --git a/unitree_api/msg/Response.msg b/unitree_api/msg/Response.msg new file mode 100755 index 0000000..2036630 --- /dev/null +++ b/unitree_api/msg/Response.msg @@ -0,0 +1,3 @@ +ResponseHeader header +string data +int8[] binary diff --git a/unitree_api/msg/ResponseHeader.msg b/unitree_api/msg/ResponseHeader.msg new file mode 100755 index 0000000..3d51649 --- /dev/null +++ b/unitree_api/msg/ResponseHeader.msg @@ -0,0 +1,2 @@ +RequestIdentity identity +ResponseStatus status diff --git a/unitree_api/msg/ResponseStatus.msg b/unitree_api/msg/ResponseStatus.msg new file mode 100755 index 0000000..1d379fa --- /dev/null +++ b/unitree_api/msg/ResponseStatus.msg @@ -0,0 +1 @@ +int32 code \ No newline at end of file diff --git a/unitree_api/package.xml b/unitree_api/package.xml new file mode 100755 index 0000000..8211947 --- /dev/null +++ b/unitree_api/package.xml @@ -0,0 +1,24 @@ + + + + unitree_api + 0.0.0 + TODO: Package description + Unitree +BSD 3-Clause License + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + ament_cmake + + geometry_msgs + rosidl_generator_dds_idl + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/unitree_go/CMakeLists.txt b/unitree_go/CMakeLists.txt new file mode 100755 index 0000000..c369295 --- /dev/null +++ b/unitree_go/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.5) +project(unitree_go) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rosidl_generator_dds_idl REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/AudioData.msg" + "msg/BmsCmd.msg" + "msg/BmsState.msg" + "msg/Error.msg" + "msg/HeightMap.msg" + "msg/IMUState.msg" + "msg/InterfaceConfig.msg" + "msg/LidarState.msg" + "msg/LowCmd.msg" + "msg/LowState.msg" + "msg/MotorCmd.msg" + "msg/MotorCmds.msg" + "msg/MotorState.msg" + "msg/MotorStates.msg" + "msg/PathPoint.msg" + "msg/Req.msg" + "msg/Res.msg" + "msg/SportModeCmd.msg" + "msg/SportModeState.msg" + "msg/TimeSpec.msg" + "msg/UwbState.msg" + "msg/UwbSwitch.msg" + "msg/WirelessController.msg" + DEPENDENCIES geometry_msgs +) + +rosidl_generate_dds_interfaces( + ${rosidl_generate_interfaces_TARGET}__dds_connext_idl + IDL_TUPLES ${rosidl_generate_interfaces_IDL_TUPLES} + OUTPUT_SUBFOLDERS "dds_connext" +) +add_dependencies( + ${PROJECT_NAME} + ${PROJECT_NAME}__dds_connext_idl +) + + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/go2_interfaces/msg/AudioData.msg b/unitree_go/msg/AudioData.msg similarity index 100% rename from go2_interfaces/msg/AudioData.msg rename to unitree_go/msg/AudioData.msg diff --git a/go2_interfaces/msg/BmsCmd.msg b/unitree_go/msg/BmsCmd.msg similarity index 100% rename from go2_interfaces/msg/BmsCmd.msg rename to unitree_go/msg/BmsCmd.msg diff --git a/go2_interfaces/msg/BmsState.msg b/unitree_go/msg/BmsState.msg similarity index 100% rename from go2_interfaces/msg/BmsState.msg rename to unitree_go/msg/BmsState.msg diff --git a/go2_interfaces/msg/Error.msg b/unitree_go/msg/Error.msg similarity index 100% rename from go2_interfaces/msg/Error.msg rename to unitree_go/msg/Error.msg diff --git a/go2_interfaces/msg/HeightMap.msg b/unitree_go/msg/HeightMap.msg similarity index 100% rename from go2_interfaces/msg/HeightMap.msg rename to unitree_go/msg/HeightMap.msg diff --git a/go2_interfaces/msg/IMUState.msg b/unitree_go/msg/IMUState.msg similarity index 100% rename from go2_interfaces/msg/IMUState.msg rename to unitree_go/msg/IMUState.msg diff --git a/go2_interfaces/msg/InterfaceConfig.msg b/unitree_go/msg/InterfaceConfig.msg similarity index 100% rename from go2_interfaces/msg/InterfaceConfig.msg rename to unitree_go/msg/InterfaceConfig.msg diff --git a/go2_interfaces/msg/LidarState.msg b/unitree_go/msg/LidarState.msg similarity index 100% rename from go2_interfaces/msg/LidarState.msg rename to unitree_go/msg/LidarState.msg diff --git a/go2_interfaces/msg/LowCmd.msg b/unitree_go/msg/LowCmd.msg similarity index 100% rename from go2_interfaces/msg/LowCmd.msg rename to unitree_go/msg/LowCmd.msg diff --git a/go2_interfaces/msg/LowState.msg b/unitree_go/msg/LowState.msg similarity index 100% rename from go2_interfaces/msg/LowState.msg rename to unitree_go/msg/LowState.msg diff --git a/go2_interfaces/msg/MotorCmd.msg b/unitree_go/msg/MotorCmd.msg similarity index 100% rename from go2_interfaces/msg/MotorCmd.msg rename to unitree_go/msg/MotorCmd.msg diff --git a/go2_interfaces/msg/MotorCmds.msg b/unitree_go/msg/MotorCmds.msg similarity index 100% rename from go2_interfaces/msg/MotorCmds.msg rename to unitree_go/msg/MotorCmds.msg diff --git a/go2_interfaces/msg/MotorState.msg b/unitree_go/msg/MotorState.msg similarity index 100% rename from go2_interfaces/msg/MotorState.msg rename to unitree_go/msg/MotorState.msg diff --git a/go2_interfaces/msg/MotorStates.msg b/unitree_go/msg/MotorStates.msg similarity index 100% rename from go2_interfaces/msg/MotorStates.msg rename to unitree_go/msg/MotorStates.msg diff --git a/go2_interfaces/msg/PathPoint.msg b/unitree_go/msg/PathPoint.msg similarity index 100% rename from go2_interfaces/msg/PathPoint.msg rename to unitree_go/msg/PathPoint.msg diff --git a/go2_interfaces/msg/Req.msg b/unitree_go/msg/Req.msg similarity index 100% rename from go2_interfaces/msg/Req.msg rename to unitree_go/msg/Req.msg diff --git a/go2_interfaces/msg/Res.msg b/unitree_go/msg/Res.msg similarity index 100% rename from go2_interfaces/msg/Res.msg rename to unitree_go/msg/Res.msg diff --git a/go2_interfaces/msg/SportModeCmd.msg b/unitree_go/msg/SportModeCmd.msg similarity index 100% rename from go2_interfaces/msg/SportModeCmd.msg rename to unitree_go/msg/SportModeCmd.msg diff --git a/go2_interfaces/msg/SportModeState.msg b/unitree_go/msg/SportModeState.msg similarity index 100% rename from go2_interfaces/msg/SportModeState.msg rename to unitree_go/msg/SportModeState.msg diff --git a/go2_interfaces/msg/TimeSpec.msg b/unitree_go/msg/TimeSpec.msg similarity index 100% rename from go2_interfaces/msg/TimeSpec.msg rename to unitree_go/msg/TimeSpec.msg diff --git a/go2_interfaces/msg/UwbState.msg b/unitree_go/msg/UwbState.msg similarity index 100% rename from go2_interfaces/msg/UwbState.msg rename to unitree_go/msg/UwbState.msg diff --git a/go2_interfaces/msg/UwbSwitch.msg b/unitree_go/msg/UwbSwitch.msg similarity index 100% rename from go2_interfaces/msg/UwbSwitch.msg rename to unitree_go/msg/UwbSwitch.msg diff --git a/go2_interfaces/msg/WirelessController.msg b/unitree_go/msg/WirelessController.msg similarity index 100% rename from go2_interfaces/msg/WirelessController.msg rename to unitree_go/msg/WirelessController.msg diff --git a/unitree_go/package.xml b/unitree_go/package.xml new file mode 100755 index 0000000..d964f07 --- /dev/null +++ b/unitree_go/package.xml @@ -0,0 +1,24 @@ + + + + unitree_go + 0.0.1 + Communication interfaces of the unitree go 2 robots + Unitree + BSD 3-Clause License + + rosidl_default_generators + rosidl_default_runtime + rosidl_generator_dds_idl + rosidl_interface_packages + ament_cmake + + geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + +