From 7d3ca20c9d2dc8555ffea48fd487cd3f51feaef3 Mon Sep 17 00:00:00 2001 From: Kishan Sawant Date: Tue, 10 Dec 2024 10:00:25 +0100 Subject: [PATCH] add eddie_platform_control --- CMakeLists.txt | 172 +++++++-------- README.md | 2 + include/PackageName/temp.hpp | 20 -- include/PackageName/template.h | 30 --- include/eddie_platform_control/KeloDrive.h | 162 +++++++++++++++ .../PlatformController.h | 189 +++++++++++++++++ launch/kelo_platform_controller_gz.launch.py | 25 +++ package.xml | 33 +++ src/CMakeLists.txt | 16 -- src/KeloDrive.cpp | 61 ++++++ src/PlatformController.cpp | 196 ++++++++++++++++++ src/example/CMakeLists.txt | 10 - src/example/temp_ex.c | 10 - src/main.cpp | 117 +++++++++++ src/template.c | 5 - 15 files changed, 858 insertions(+), 190 deletions(-) delete mode 100644 include/PackageName/temp.hpp delete mode 100644 include/PackageName/template.h create mode 100644 include/eddie_platform_control/KeloDrive.h create mode 100644 include/eddie_platform_control/PlatformController.h create mode 100644 launch/kelo_platform_controller_gz.launch.py create mode 100644 package.xml delete mode 100644 src/CMakeLists.txt create mode 100644 src/KeloDrive.cpp create mode 100644 src/PlatformController.cpp delete mode 100644 src/example/CMakeLists.txt delete mode 100644 src/example/temp_ex.c create mode 100644 src/main.cpp delete mode 100644 src/template.c diff --git a/CMakeLists.txt b/CMakeLists.txt index d15f3c9..d6b5065 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,104 +1,78 @@ -cmake_minimum_required(VERSION 3.20) +cmake_minimum_required(VERSION 3.8) +project(eddie_platform_control) -set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") - -project(PackageName) - -include(GNUInstallDirs) - -set(CMAKE_INSTALL_DIR ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}) - -option(ENABLE_DOC "Build documentation" Off) -option(ENABLE_TESTS "Build unit tests" Off) -option(ENABLE_PACKAGE_REGISTRY "Add this package to CMake's package registry" Off) - -add_subdirectory(src) - -if(ENABLE_TESTS) - message(STATUS "Building tests") - enable_testing() - add_subdirectory(test) -endif() - -if(ENABLE_DOC) - # Find Doxygen package - find_package(Doxygen REQUIRED) - - # Set Doxygen variables - set(DOXYGEN_GENERATE_HTML YES) # Enable HTML documentation generation - set(DOXYGEN_GENERATE_XML NO) # Disable XML output (optional, depends on your needs) - set(DOXYGEN_EXCLUDE "test/*") # Optionally exclude test folders from documentation - - # Set the paths for the input sources and the output directory - set(DOXYGEN_INPUT_DIR "${CMAKE_SOURCE_DIR}/src") # Path to source code directory - set(DOXYGEN_INCLUDE_DIR "${CMAKE_SOURCE_DIR}/include") # Path to include directory - set(DOXYGEN_README "${CMAKE_SOURCE_DIR}/README.md") # Path to README.md - set(DOXYGEN_OUTPUT_DIR "${CMAKE_BINARY_DIR}/docs") # Output directory for the docs - set(HTML_EXTRA_STYLESHEET "${CMAKE_SOURCE_DIR}/docs/styles/doxy.css") # Path to custom CSS - set(HTML_HEADER "${CMAKE_SOURCE_DIR}/docs/styles/doxy_header.html") # Path to custom header - - # Ensure that the output directory exists - file(MAKE_DIRECTORY ${DOXYGEN_OUTPUT_DIR}) - - # Provide path to Doxygen config file - set(DOXYGEN_CONFIG_FILE "${CMAKE_SOURCE_DIR}/Doxyfile.in") # Path to your Doxygen configuration - - # Check if Doxygen configuration file exists - if(EXISTS "${DOXYGEN_CONFIG_FILE}") - message(STATUS "Doxygen config file found: ${DOXYGEN_CONFIG_FILE}") - else() - message(FATAL_ERROR "Doxygen config file not found!") - endif() - - # Modify the Doxyfile variables in the CMakeLists.txt - configure_file(${CMAKE_SOURCE_DIR}/Doxyfile.in ${CMAKE_BINARY_DIR}/Doxyfile @ONLY) - - # Add custom target to generate docs using Doxygen - add_custom_target( - docs - COMMAND ${DOXYGEN_EXECUTABLE} ${CMAKE_BINARY_DIR}/Doxyfile - WORKING_DIRECTORY ${CMAKE_BINARY_DIR} - COMMENT "Generating API documentation with Doxygen" - VERBATIM) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) endif() -# Export this package to CMake's package registry -if(ENABLE_PACKAGE_REGISTRY) - export(PACKAGE ${PROJECT_NAME}) +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(gazebo_msgs REQUIRED) +find_package(kelo_tulip REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(Boost REQUIRED COMPONENTS thread) + +# Optional documentation build +option(BUILD_DOC "Build documentation" OFF) +if(BUILD_DOC) + find_package(Doxygen) + if(DOXYGEN_FOUND) + set(DOXYGEN_IN ${CMAKE_CURRENT_SOURCE_DIR}/config/doxygen/Doxyfile.in) + set(DOXYGEN_OUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) + + configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT} @ONLY) + + add_custom_target(doc_doxygen ALL + COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_OUT} + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + COMMENT "Generating API documentation with Doxygen" + VERBATIM) + else() + message("Doxygen needs to be installed to generate documentation") + endif() endif() -include(CMakePackageConfigHelpers) -configure_package_config_file( - ${PROJECT_NAME}-config.cmake.in "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake" - INSTALL_DESTINATION ${CMAKE_INSTALL_DIR}) - -# Generate the version file for the config file -write_basic_package_version_file( - "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake" - VERSION "0.1.0" - COMPATIBILITY AnyNewerVersion) - -# Install cmake configuration and package version -install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake" - "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake" - DESTINATION ${CMAKE_INSTALL_DIR}) - -# Make the targets accessible from this packages's build tree -export( - EXPORT ${PROJECT_NAME}-targets - NAMESPACE ${PROJECT_NAME}:: - FILE "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-targets.cmake") - -# Make the targets accessible from this packages's install tree -install( - EXPORT ${PROJECT_NAME}-targets - NAMESPACE ${PROJECT_NAME}:: - DESTINATION ${CMAKE_INSTALL_DIR}) - -# Install all public header files -install( - DIRECTORY include/ - DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} - FILES_MATCHING - PATTERN "*.h" - PATTERN "*.hpp") +# Include directories +include_directories( + include + ${kelo_tulip_INCLUDE_DIRS} +) + +# Executable +add_executable(kelo_gazebo_platform_controller + src/KeloDrive.cpp + src/PlatformController.cpp + src/main.cpp +) + +# Link libraries +ament_target_dependencies(kelo_gazebo_platform_controller + rclcpp + visualization_msgs + tf2 + tf2_geometry_msgs + nav_msgs + kelo_tulip + Boost + gazebo_msgs +) + +# Install directories +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +# Install executable +install(TARGETS + kelo_gazebo_platform_controller + DESTINATION lib/${PROJECT_NAME} +) + +# Necessary for ament_package() +ament_package() \ No newline at end of file diff --git a/README.md b/README.md index ac605cc..7d77bea 100644 --- a/README.md +++ b/README.md @@ -13,6 +13,8 @@ This repository is a template for C/C++ packages. - clang-format - clang-tidy - [pre-commit](https://pre-commit.com/) +- kelo_tulip +- eddie_gazebo #### Install prerequisites diff --git a/include/PackageName/temp.hpp b/include/PackageName/temp.hpp deleted file mode 100644 index 606e469..0000000 --- a/include/PackageName/temp.hpp +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef PACKAGENAME_TEMP_HPP -#define PACKAGENAME_TEMP_HPP - -class Temp -{ - public: - // constructor - explicit Temp(int pVar); - - void print(); - - // member variables - int mVar; - - private: - // class variables - static const int C_COUNT; -}; - -#endif// PACKAGENAME_TEMP_HPP diff --git a/include/PackageName/template.h b/include/PackageName/template.h deleted file mode 100644 index c5a7cc7..0000000 --- a/include/PackageName/template.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef PACKAGENAME_TEMPLATE_H -#define PACKAGENAME_TEMPLATE_H - -/** - * @brief Add two integers - * - * @param pA First integer - * @param pB Second integer - * @return int Sum of the two integers - */ -int add(int pA, int pB); - -/** - * @brief Subtract two integers - * - * @param pA First integer - * @param pB Second integer - * @return int Difference of the two integers - */ -int subtract(int pA, int pB); - -int multiply(int pA, int pB) -{ - int result = 0; - int run = 0; - for (int i = 0; i < pB; i++) { result = add(result, pA); } - return result; -} - -#endif// PACKAGENAME_TEMPLATE_H diff --git a/include/eddie_platform_control/KeloDrive.h b/include/eddie_platform_control/KeloDrive.h new file mode 100644 index 0000000..83cedab --- /dev/null +++ b/include/eddie_platform_control/KeloDrive.h @@ -0,0 +1,162 @@ +/****************************************************************************** + * Copyright (c) 2021 + * KELO Robotics GmbH + * + * Author: + * Sushant Chavan + * + * + * This software is published under a dual-license: GNU Lesser General Public + * License LGPL 2.1 and BSD license. The dual-license implies that users of this + * code may choose which terms they prefer. + * + * 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 Locomotec nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 2.1 of the + * License, or (at your option) any later version or the BSD license. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL and the BSD license for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL and BSD license along with this program. + * + ******************************************************************************/ + +#ifndef KELO_DRIVE_HPP +#define KELO_DRIVE_HPP + +#include +#include +#include +#include +#include + +/** + * @brief Class to store information about and manage an individual Kelo drive + * that is part of a KELO platform + * + */ +class KeloDrive { + public: + /** + * @brief Construct a new Kelo Drive object + * + * @param name Name of the Kelo drive + * @param xPos Position of the kelo drive along the x-axis of the base_link + * @param yPos Position of the kelo drive along the y-axis of the base_link + * @param zPos Position of the kelo drive along the z-axis of the base_link + * @param pivotOrientation Initial orientation of the kelo drive pivot with + * respect to the base_link + */ + KeloDrive(const std::string& name, double xPos, + double yPos, double zPos, double pivotOrientation); + + /** + * @brief Destroy the Kelo Drive object + * + */ + virtual ~KeloDrive() {} + + /** + * @brief Get the position of the kelo drive w.r.t base_link + * + * @param xPos + * @param yPos + * @param zPos + */ + void getPos(double& xPos, double& yPos, double& zPos) const; + + /** + * @brief Set the latest pivot orientation of the kelo drive + * + * @param orientation Latest pivot orientation w.r.t base_link + */ + void setPivotOrientation(double orientation); + + /** + * @brief Get the latest pivot orientation of the kelo drive + * + * @return double Latest pivot orientation w.r.t base_link + */ + double getPivotOrientation() const { return pivot_orientation_; } + + /** + * @brief Get an RViz marker object representing the pose of the kelo drive + * pivot for debugging purposes + * + * @return visualization_msgs::msg::Marker marker representing current pose of + * the kelo drive pivot + */ + // visualization_msgs::msg::Marker getPivotMarker() const; + + /** + * @brief Set the desired angular velocities for each of the two hub wheels + * of the kelo drive + * + * @param leftAngVel Angular velocity for the left hub wheel in rad/s + * @param rightAngVel Angular velocity for the right hub wheel in rad/s + */ + // void setHubWheelVelocities(double leftAngVel, double rightAngVel); + + protected: + /** + * @brief Name of the kelo drive + * + */ + std::string name_; + + /** + * @brief Position of the kelo drive along the x-axis of the base_link + * + */ + double x_pos_; + + /** + * @brief Position of the kelo drive along the y-axis of the base_link + * + */ + double y_pos_; + + /** + * @brief Position of the kelo drive along the z-axis of the base_link + * + */ + double z_pos_; + + /** + * @brief Orientation of the kelo drive pivot with respect to the base_link + * + */ + double pivot_orientation_; + + /** + * @brief ROS 2 publishers for setting the desired velocity for the left hub + * wheel + * + */ + // rclcpp::Publisher::SharedPtr left_hub_wheel_vel_pub_; + + /** + * @brief ROS 2 publishers for setting the desired velocity for the right hub + * wheel + * + */ + // rclcpp::Publisher::SharedPtr right_hub_wheel_vel_pub_; +}; + +#endif // KELO_DRIVE_HPP diff --git a/include/eddie_platform_control/PlatformController.h b/include/eddie_platform_control/PlatformController.h new file mode 100644 index 0000000..a774646 --- /dev/null +++ b/include/eddie_platform_control/PlatformController.h @@ -0,0 +1,189 @@ +/****************************************************************************** + * Copyright (c) 2021 + * KELO Robotics GmbH + * + * Author: + * Sushant Chavan + * Walter Nowak + * + * + * This software is published under a dual-license: GNU Lesser General Public + * License LGPL 2.1 and BSD license. The dual-license implies that users of this + * code may choose which terms they prefer. + * + * 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 Locomotec nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 2.1 of the + * License, or (at your option) any later version or the BSD license. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL and the BSD license for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL and BSD license along with this program. + * + ******************************************************************************/ +#ifndef PLATFORM_CONTROLLER_HPP +#define PLATFORM_CONTROLLER_HPP + +#include +#include +#include +#include +#include +#include +#include "tf2_ros/buffer.h" + +#include +#include "KeloDrive.h" + +/** + * @brief Velocity controller for gazebo simulations of arbitrary + * KELO platform configurations + */ +class PlatformController : public rclcpp::Node { +public: + /** + * @brief Construct a new KELO Platform Controller object + */ + PlatformController(const std::string& node_name); + + /** + * @brief Set the linear and angular velocities for the KELO platform + * + * @param vx Linear velocity (in m/s) along the positive x axis in robot frame + * @param vy Linear velocity (in m/s) along the positive y axis in robot frame + * @param va Angular velocity in (rad/s) around the positive z axis in robot frame + */ + void setCmdVel(double vx, double vy, double va); + + /** + * @brief Set the maximum linear and angular velocities for the KELO platform + * + * @param linearVel Max linear velocity the KELO platform could achieve + * @param angularVel Max angular velocity the KELO platform could achieve + */ + void setMaxPlatformVelocity(double linearVel, double angularVel); + + /** + * @brief Controller step which computes and sets the desired hub wheel velocities + */ + void step(); + + /** + * @brief Publish RViz markers for each active wheel's pivot pose + */ + void publishPivotMarkers() const; + + /** + * @brief Set the frequency at which odometry messages are published + * + * @param frequency Frequency (in Hz) at which odometry messages should be published + */ + void setOdomFrequency(double frequency); + + /** + * @brief Publish the latest odometry received from Gazebo on '/odom' topic + */ + void publishOdom(); + + /** + * @brief Publish the latest transform from odom to base_link on the '/tf' topic + */ + void publishOdomToBaseLinkTF(); + + /** + * @brief Set the wheel setpoint velocities for all the hub wheels in the KELO platform + */ + void setAllHubWheelVelocities(const std_msgs::msg::Float64MultiArray& msg); + + +protected: + /** + * @brief Callback function to receive and process the robot joint states from Gazebo + * + * @param msg ROS message from Gazebo with the Joint state information + */ + void jointStatesCallBack(const sensor_msgs::msg::JointState::SharedPtr msg); + + /** + * @brief Callback function to receive and process the robot link states from Gazebo + * + * @param msg ROS message from Gazebo with the Link state information + */ + void gazeboLinkStatesCallBack(const gazebo_msgs::msg::LinkStates::SharedPtr msg); + + /** + * @brief Initialize the Kelo drive data structures and the Velocity controller + * + * @param pivotJointData Map of Kelo drive name vs its latest pivot orientation + */ + void initDrives(const std::map& pivotJointData); + + /** + * @brief Set the Pivot Orientations for every Kelo drive in the KELO platform + * + * @param pivotJointData Map of Kelo drive name vs the latest pivot orientation + */ + void setPivotOrientations(const std::map& pivotJointData); + + /** + * @brief Extract the pivot name from the pivot joint name + * + * @param jointName Joint name of the Kelo drive pivot + * @return std::string Name of the pivot + */ + std::string getPivotName(const std::string& jointName); + + // ROS2 Specific Members + std::unique_ptr _tfBuffer; + std::unique_ptr _tfListener; + + rclcpp::Subscription::SharedPtr _jointStatesSubscriber; + rclcpp::Publisher::SharedPtr base_velocity_control_pub_; + // rclcpp::Subscription::SharedPtr _gazeboLinkStatesSubscriber; + // rclcpp::Publisher::SharedPtr _odomPublisher; + // rclcpp::Publisher::SharedPtr _odomTFPublisher; + // rclcpp::Publisher::SharedPtr _pivotMarkersPublisher; + // rclcpp::TimerBase::SharedPtr _odomTimer; + + // Existing Members + std::map _drives; + bool _initialized{false}; + + double _cmdVelX{0.0}; + double _cmdVelY{0.0}; + double _cmdVelA{0.0}; + + std::map drives_map = { + // Note: the order of the drives should be same as the order of controllers in eddie_controllers.yaml (eddie_gazebo) + // Assumption: for each wheel unit, first the left wheel and then the right wheel is considered in the controller configuration file + + {"eddie_front_left", 0}, + {"eddie_rear_left", 1}, + {"eddie_rear_right", 2}, + {"eddie_front_right", 3} + }; + + // nav_msgs::msg::Odometry _odomMsg; + // rclcpp::Duration _odomDuration{rclcpp::Duration::from_seconds(0.1)}; + + kelo::VelocityPlatformController _controller; + std::map _wheelConfigs; +}; + +#endif // PLATFORM_CONTROLLER_HPP diff --git a/launch/kelo_platform_controller_gz.launch.py b/launch/kelo_platform_controller_gz.launch.py new file mode 100644 index 0000000..8891598 --- /dev/null +++ b/launch/kelo_platform_controller_gz.launch.py @@ -0,0 +1,25 @@ +import launch +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +import os + +def generate_launch_description(): + return LaunchDescription([ + # Declare the parameter for platform_max_lin_vel if needed + DeclareLaunchArgument('platform_max_lin_vel', default_value='1.0', description='Max Linear Velocity'), + DeclareLaunchArgument('platform_max_ang_vel', default_value='1.0', description='Max Angular Velocity'), + + # Start the KeloGazeboController node + Node( + package='eddie_platform_control', + executable='kelo_gazebo_platform_controller', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'platform_max_lin_vel': LaunchConfiguration('platform_max_lin_vel'), + 'platform_max_ang_vel': LaunchConfiguration('platform_max_ang_vel'), + }], + ), + ]) \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..d2b89ba --- /dev/null +++ b/package.xml @@ -0,0 +1,33 @@ + + + eddie_platform_control + 1.0.0 + Gazebo simulations for KELO robots + + Walter Nowak + Sebastian Blumenthal + + LGPL2.1/BSD + + Sushant Chavan + Dharmin B. + + ament_cmake + ament_index_cpp + nav_msgs + gazebo_msgs + rclcpp + eddie_description + robot_state_publisher + rviz + kelo_tulip + joint_state_publisher + gazebo_ros + xacro + gazebo_ros_control + geometry_msgs + + + ament_cmake + + \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt deleted file mode 100644 index fc7860a..0000000 --- a/src/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -add_library(PN_test_lib SHARED template.c) - -target_include_directories( - PN_test_lib - PUBLIC $ - $ - PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) - -install( - TARGETS PN_test_lib - EXPORT ${PROJECT_NAME}-targets - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) - -add_subdirectory(example) diff --git a/src/KeloDrive.cpp b/src/KeloDrive.cpp new file mode 100644 index 0000000..1fd51fc --- /dev/null +++ b/src/KeloDrive.cpp @@ -0,0 +1,61 @@ +/****************************************************************************** + * Copyright (c) 2021 + * KELO Robotics GmbH + * + * Author: + * Sushant Chavan + * + * + * This software is published under a dual-license: GNU Lesser General Public + * License LGPL 2.1 and BSD license. The dual-license implies that users of this + * code may choose which terms they prefer. + * + * 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 Locomotec nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 2.1 of the + * License, or (at your option) any later version or the BSD license. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL and the BSD license for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL and BSD license along with this program. + * + ******************************************************************************/ + +#include +#include +#include +#include +#include "eddie_platform_control/KeloDrive.h" + +KeloDrive::KeloDrive(const std::string& name, double xPos, double yPos, double zPos, double pivotOrientation): +name_(name), +x_pos_(xPos), +y_pos_(yPos), +z_pos_(zPos), +pivot_orientation_(pivotOrientation) {} + +void KeloDrive::getPos(double& xPos, double& yPos, double& zPos) const { + xPos = x_pos_; + yPos = y_pos_; + zPos = z_pos_; +} + +void KeloDrive::setPivotOrientation(double orientation) { + pivot_orientation_ = orientation; +} \ No newline at end of file diff --git a/src/PlatformController.cpp b/src/PlatformController.cpp new file mode 100644 index 0000000..130f751 --- /dev/null +++ b/src/PlatformController.cpp @@ -0,0 +1,196 @@ +/****************************************************************************** + * Copyright (c) 2021 + * KELO Robotics GmbH + * + * Author: + * Sushant Chavan + * Walter Nowak + * + * + * This software is published under a dual-license: GNU Lesser General Public + * License LGPL 2.1 and BSD license. The dual-license implies that users of this + * code may choose which terms they prefer. + * + * 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 Locomotec nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 2.1 of the + * License, or (at your option) any later version or the BSD license. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL and the BSD license for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL and BSD license along with this program. + * + ******************************************************************************/ +#include +#include +#include +#include +#include +#include +#include + +#include "eddie_platform_control/PlatformController.h" + +PlatformController::PlatformController(const std::string& node_name) + : Node(node_name), + _cmdVelX(0.0), + _cmdVelY(0.0), + _cmdVelA(0.0), + _initialized(false) { + _tfBuffer = std::make_unique(this->get_clock()); + _tfListener = std::make_unique(*_tfBuffer); + + // Setup subscribers + _jointStatesSubscriber = this->create_subscription( + "/joint_states", + rclcpp::QoS(10).reliable().transient_local(), + std::bind(&PlatformController::jointStatesCallBack, this, std::placeholders::_1) + ); + + // Setup publishers + base_velocity_control_pub_ = this->create_publisher("/base_velocity_controller/commands", 10); + } + + void PlatformController::setCmdVel(double vx, double vy, double va) { + _cmdVelX = vx; + _cmdVelY = vy; + _cmdVelA = va; + } + + void PlatformController::setMaxPlatformVelocity(double linearVel, double angularVel) { + _controller.setPlatformMaxLinVelocity(linearVel); + _controller.setPlatformMaxAngVelocity(angularVel); + } + + void PlatformController::step() { + if (_initialized) { + _controller.setPlatformTargetVelocity(_cmdVelX, _cmdVelY, _cmdVelA); + _controller.calculatePlatformRampedVelocities(); + std_msgs::msg::Float64MultiArray message; + size_t array_size = 8; + message.data.resize(array_size); + + for (const auto& drive: _drives) { + const std::string& driveName = drive.first; + int wheelNumber = _wheelConfigs[driveName].ethercatNumber; + float left_whl_sp, right_whl_sp; + _controller.calculateWheelTargetVelocity(wheelNumber, drive.second.getPivotOrientation(), left_whl_sp, right_whl_sp); + message.data[2*wheelNumber] = 0.; //left_whl_sp; + message.data[2*wheelNumber+1] = 0.; //-right_whl_sp; + std::cout << "Drive: " << driveName << " Wheel Number: " << wheelNumber << " Left: " << left_whl_sp << " Right: " << -right_whl_sp << std::endl; + } + + setAllHubWheelVelocities(message); + // TODO: map the wheel velocities according to drive name + } + } + + void PlatformController::initDrives(const std::map& pivotJointData) { + if (!_drives.empty()) + return; + + auto now = this->get_clock()->now(); + for (auto& joint: pivotJointData) { + std::string driveName = getPivotName(joint.first); + // Eg: from "eddie_rear_right_drive_pivot_joint" to "eddie_rear_right_drive_struct_link" + std::string pivotLink = driveName + "_drive_struct_link"; + + geometry_msgs::msg::TransformStamped transform; + try { + transform = _tfBuffer->lookupTransform("eddie_base_link", pivotLink, now, rclcpp::Duration::from_seconds(2.0)); + } catch (tf2::TransformException& ex) { + RCLCPP_WARN(this->get_logger(), "Transform error: %s", ex.what()); + _drives.clear(); + return; + } + + _drives.insert(std::make_pair(driveName, + KeloDrive(driveName, + transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z, + joint.second))); + } + + std::vector wheelConfigsVector; + int wheelNumber = 0; + double zDummy = 0; + for (const auto& drive: _drives) { + kelo::WheelConfig wc; + wc.ethercatNumber = drives_map[drive.first]; + drive.second.getPos(wc.x, wc.y, zDummy); + wc.a = 0; // assume zero in simulation + _wheelConfigs[drive.first] = wc; + wheelConfigsVector.push_back(wc); + wheelNumber++; + } + _controller.initialise(wheelConfigsVector); + + _initialized = true; + RCLCPP_INFO(this->get_logger(), "Initialized %lu Kelo drives", _drives.size()); + } + + void PlatformController::jointStatesCallBack(const sensor_msgs::msg::JointState::SharedPtr msg) { + const std::vector& jointNames = msg->name; + const std::vector& jointPositions = msg->position; + + if (jointNames.empty() || jointNames.size() != jointPositions.size()) { + return; + } + + std::map pivotJointData; + std::string pivotJointNameIdentifier = "pivot_joint"; + for (size_t i = 0; i < jointNames.size(); ++i) { + std::string jointName = jointNames[i]; + if (jointName.find(pivotJointNameIdentifier) != std::string::npos) { + double pivotAngle = jointPositions[i] - (int(jointPositions[i] / (2*M_PI)) * 2 * M_PI); + pivotJointData[jointName] = pivotAngle; + } + } + + if (_drives.empty()) { + initDrives(pivotJointData); + } else { + setPivotOrientations(pivotJointData); + } + } + + void PlatformController::setPivotOrientations(const std::map& pivotJointData) { + for (const auto& joint: pivotJointData) { + std::string driveName = getPivotName(joint.first); + if (_drives.find(driveName) == _drives.end()) { + RCLCPP_ERROR(this->get_logger(), "Cannot set pivot orientation for drive %s", driveName.c_str()); + continue; + } + KeloDrive& drive = _drives.at(driveName); + drive.setPivotOrientation(joint.second); + } + } + + std::string PlatformController::getPivotName(const std::string& jointName) { + return jointName.substr(0, jointName.find("_drive_")); + } + + void PlatformController::setAllHubWheelVelocities(const std_msgs::msg::Float64MultiArray& msg) { + if (!base_velocity_control_pub_) { + RCLCPP_WARN(this->get_logger(), "Publisher is not initialized!"); + return; + } + base_velocity_control_pub_->publish(msg); + } diff --git a/src/example/CMakeLists.txt b/src/example/CMakeLists.txt deleted file mode 100644 index 9184468..0000000 --- a/src/example/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -list(APPEND CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") - -add_executable(temp_ex temp_ex.c) -target_include_directories(temp_ex PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(temp_ex PN_test_lib) - -install( - TARGETS temp_ex - EXPORT ${PROJECT_NAME}-targets - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) diff --git a/src/example/temp_ex.c b/src/example/temp_ex.c deleted file mode 100644 index def8381..0000000 --- a/src/example/temp_ex.c +++ /dev/null @@ -1,10 +0,0 @@ -#include -#include "PackageName/template.h" - -int main() -{ - int x = 10, y = 5; - printf("Addition of %d and %d is: %d\n", x, y, add(x, y)); - printf("Subtraction of %d and %d is: %d\n", x, y, subtract(x, y)); - return 0; -} diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..ebce004 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,117 @@ +/****************************************************************************** + * Copyright (c) 2021 + * KELO Robotics GmbH + * + * Author: + * Sushant Chavan + * Walter Nowak + * + * + * This software is published under a dual-license: GNU Lesser General Public + * License LGPL 2.1 and BSD license. The dual-license implies that users of this + * code may choose which terms they prefer. + * + * 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 Locomotec nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 2.1 of the + * License, or (at your option) any later version or the BSD license. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL and the BSD license for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL and BSD license along with this program. + * + ******************************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include "eddie_platform_control/PlatformController.h" + +class KeloGazeboController : public rclcpp::Node { +private: + std::shared_ptr platformController; + rclcpp::Subscription::SharedPtr cmdVelSubscriber; + rclcpp::Subscription::SharedPtr joySubscriber; + rclcpp::TimerBase::SharedPtr controlTimer; + rclcpp::executors::MultiThreadedExecutor* executor; + + void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { + if (platformController) { + platformController->setCmdVel(msg->linear.x, msg->linear.y, msg->angular.z); + } + } + + void joyCallback(const sensor_msgs::msg::Joy::SharedPtr joy) { + if (platformController) { + platformController->setCmdVel(joy->axes[1], joy->axes[0], joy->axes[2]); + } + } + + void controlLoop() { + platformController->step(); + } + +public: + KeloGazeboController(const std::string& node_name, rclcpp::executors::MultiThreadedExecutor* exec) + : Node(node_name), executor(exec) { + + cmdVelSubscriber = this->create_subscription( + "/cmd_vel", 10, + std::bind(&KeloGazeboController::cmdVelCallback, this, std::placeholders::_1) + ); + + joySubscriber = this->create_subscription( + "/joy", 1000, + std::bind(&KeloGazeboController::joyCallback, this, std::placeholders::_1) + ); + + // Load max linear velocity + double platformMaxLinVel = this->declare_parameter("platform_max_lin_vel", 1.0); + + // Load max angular velocity + double platformMaxAngVel = this->declare_parameter("platform_max_ang_vel", 1.0); + + // Initialize platform controller + platformController = std::make_shared("platform_controller"); + if (executor) { + executor->add_node(platformController); + } + platformController->setMaxPlatformVelocity(platformMaxLinVel, platformMaxAngVel); + + // Create timer for control loop (50Hz) + controlTimer = this->create_wall_timer( + std::chrono::milliseconds(20), + std::bind(&KeloGazeboController::controlLoop, this) + ); + } +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared("kelo_gazebo_controller", &executor); + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/template.c b/src/template.c deleted file mode 100644 index 1fc9c57..0000000 --- a/src/template.c +++ /dev/null @@ -1,5 +0,0 @@ -#include "PackageName/template.h" - -int add(int pA, int pB) { return pA + pB; } - -int subtract(int pA, int pB) { return pA - pB; }