Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enable ros iron #139

Merged
merged 5 commits into from
Sep 21, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 20 additions & 0 deletions .github/workflows/industrial_ci_iron_action.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
name: Iron

on: [push, pull_request]


jobs:
industrial_ci:
env:
BEFORE_BUILD_TARGET_WORKSPACE: '.github/script/install_mujoco.sh'
strategy:
fail-fast: false
matrix:
env:
- {ROS_DISTRO: iron, ROS_REPO: testing}
- {ROS_DISTRO: iron, ROS_REPO: main}
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v1
- uses: 'ros-industrial/industrial_ci@master'
env: ${{matrix.env}}
1 change: 1 addition & 0 deletions README.md
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
![build badge](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/actions/workflows/industrial_ci_foxy_action.yml/badge.svg)
![build badge](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/actions/workflows/industrial_ci_galactic_action.yml/badge.svg)
![build badge](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/actions/workflows/industrial_ci_humble_action.yml/badge.svg)
![build badge](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/actions/workflows/industrial_ci_iron_action.yml/badge.svg)
[![License](https://img.shields.io/badge/License-BSD_3--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause)

---
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ class CartesianComplianceController
public:
CartesianComplianceController();

#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
virtual LifecycleNodeInterface::CallbackReturn on_init() override;
#elif defined CARTESIAN_CONTROLLERS_FOXY
virtual controller_interface::return_type init(const std::string & controller_name) override;
Expand All @@ -91,7 +91,7 @@ class CartesianComplianceController
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state) override;

#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override;
#elif defined CARTESIAN_CONTROLLERS_FOXY
controller_interface::return_type update() override;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ CartesianComplianceController::CartesianComplianceController()
{
}

#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianComplianceController::on_init()
{
using TYPE = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
Expand Down Expand Up @@ -140,7 +140,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cartes
return TYPE::SUCCESS;
}

#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
controller_interface::return_type CartesianComplianceController::update(const rclcpp::Time& time,
const rclcpp::Duration& period)
#elif defined CARTESIAN_CONTROLLERS_FOXY
Expand Down
8 changes: 5 additions & 3 deletions cartesian_controller_base/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -7,15 +7,17 @@ set(ADDITIONAL_COMPILE_OPTIONS -Wall -Wextra -Wpedantic -Wno-unused-parameter)
add_compile_options(${ADDITIONAL_COMPILE_OPTIONS})

# Use CMake to pass the current ROS_DISTRO via variables into a preprocessor template.
# We then include this file and switch between the API for foxy and galactic.
if($ENV{ROS_DISTRO} STREQUAL "humble")
# We then include this file and switch between the different APIs.
if($ENV{ROS_DISTRO} STREQUAL "iron")
set(CARTESIAN_CONTROLLERS_IRON TRUE)
elseif($ENV{ROS_DISTRO} STREQUAL "humble")
set(CARTESIAN_CONTROLLERS_HUMBLE TRUE)
elseif($ENV{ROS_DISTRO} STREQUAL "galactic")
set(CARTESIAN_CONTROLLERS_GALACTIC TRUE)
elseif($ENV{ROS_DISTRO} STREQUAL "foxy")
set(CARTESIAN_CONTROLLERS_FOXY TRUE)
else()
message(WARNING "ROS2 version must be {foxy|galactic|humble}")
message(WARNING "ROS2 version must be {iron|humble|galactic|foxy}")
endif()
configure_file(include/cartesian_controller_base/ROS2VersionConfig.h.in ROS2VersionConfig.h)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ class DampedLeastSquaresSolver : public IKSolver
*
* \return True, if everything went well
*/
#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
#else
bool init(std::shared_ptr<rclcpp::Node> nh,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ class ForwardDynamicsSolver : public IKSolver
*
* @return True, if everything went well
*/
#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
#else
bool init(std::shared_ptr<rclcpp::Node> nh,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,7 @@ class IKSolver
*
* @return True, if everything went well
*/
#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
virtual bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
#else
virtual bool init(std::shared_ptr<rclcpp::Node> nh,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ class JacobianTransposeSolver : public IKSolver
*
* \return True, if everything went well
*/
#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> /*nh*/,
#else
bool init(std::shared_ptr<rclcpp::Node> /*nh*/,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ class PDController
PDController();
~PDController();

#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
void init(const std::string& params, std::shared_ptr<rclcpp_lifecycle::LifecycleNode> handle);
#else
void init(const std::string& params, std::shared_ptr<rclcpp::Node> handle);
Expand All @@ -73,7 +73,7 @@ class PDController
double operator()(const double& error, const rclcpp::Duration& period);

private:
#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> m_handle;
#else
std::shared_ptr<rclcpp::Node> m_handle; ///< handle for dynamic parameter interaction
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,3 +4,4 @@
#cmakedefine CARTESIAN_CONTROLLERS_FOXY
#cmakedefine CARTESIAN_CONTROLLERS_GALACTIC
#cmakedefine CARTESIAN_CONTROLLERS_HUMBLE
#cmakedefine CARTESIAN_CONTROLLERS_IRON
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@ class SelectivelyDampedLeastSquaresSolver : public IKSolver
*
* \return True, if everything went well
*/
#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
#else
bool init(std::shared_ptr<rclcpp::Node> nh,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ class SpatialPDController
public:
SpatialPDController();

#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> params);
#else
bool init(std::shared_ptr<rclcpp::Node> params);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ class CartesianControllerBase : public controller_interface::ControllerInterface

virtual controller_interface::InterfaceConfiguration state_interface_configuration() const override;

#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
virtual LifecycleNodeInterface::CallbackReturn on_init() override;
#elif defined CARTESIAN_CONTROLLERS_FOXY
virtual controller_interface::return_type init(const std::string & controller_name) override;
Expand Down
2 changes: 1 addition & 1 deletion cartesian_controller_base/src/DampedLeastSquaresSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ namespace cartesian_controller_base{
return control_cmd;
}

#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
bool DampedLeastSquaresSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
#else
bool DampedLeastSquaresSolver::init(std::shared_ptr<rclcpp::Node> nh,
Expand Down
2 changes: 1 addition & 1 deletion cartesian_controller_base/src/ForwardDynamicsSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ namespace cartesian_controller_base{
}


#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
bool ForwardDynamicsSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
#else
bool ForwardDynamicsSolver::init(std::shared_ptr<rclcpp::Node> nh,
Expand Down
2 changes: 1 addition & 1 deletion cartesian_controller_base/src/IKSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ namespace cartesian_controller_base{
}


#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
bool IKSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> /*nh*/,
#else
bool IKSolver::init(std::shared_ptr<rclcpp::Node> /*nh*/,
Expand Down
2 changes: 1 addition & 1 deletion cartesian_controller_base/src/JacobianTransposeSolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ namespace cartesian_controller_base{
return control_cmd;
}

#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
bool JacobianTransposeSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
#else
bool JacobianTransposeSolver::init(std::shared_ptr<rclcpp::Node> nh,
Expand Down
2 changes: 1 addition & 1 deletion cartesian_controller_base/src/PDController.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ PDController::~PDController()
}


#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
void PDController::init(const std::string& params, std::shared_ptr<rclcpp_lifecycle::LifecycleNode> handle)
#else
void PDController::init(const std::string& params, std::shared_ptr<rclcpp::Node> handle)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ namespace cartesian_controller_base{
return control_cmd;
}

#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
bool SelectivelyDampedLeastSquaresSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
#else
bool SelectivelyDampedLeastSquaresSolver::init(std::shared_ptr<rclcpp::Node> nh,
Expand Down
2 changes: 1 addition & 1 deletion cartesian_controller_base/src/SpatialPDController.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ ctrl::Vector6D SpatialPDController::operator()(const ctrl::Vector6D& error, cons
return m_cmd;
}

#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
bool SpatialPDController::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> handle)
#else
bool SpatialPDController::init(std::shared_ptr<rclcpp::Node> handle)
Expand Down
4 changes: 2 additions & 2 deletions cartesian_controller_base/src/cartesian_controller_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ controller_interface::InterfaceConfiguration CartesianControllerBase::state_inte
return conf;
}

#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn CartesianControllerBase::on_init()
{
if (!m_initialized)
Expand Down Expand Up @@ -390,7 +390,7 @@ void CartesianControllerBase::writeJointControlCmds()
RCLCPP_ERROR(get_node()->get_logger(),
"NaN detected in internal model. It's unlikely to recover from this. Shutting down.");

#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
get_node()->shutdown();
#elif defined CARTESIAN_CONTROLLERS_FOXY || defined CARTESIAN_CONTROLLERS_GALACTIC
this->shutdown();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ class MotionControlHandle : public controller_interface::ControllerInterface
MotionControlHandle();
~MotionControlHandle();

#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
virtual LifecycleNodeInterface::CallbackReturn on_init() override;
#elif defined CARTESIAN_CONTROLLERS_FOXY
virtual controller_interface::return_type init(const std::string & controller_name) override;
Expand All @@ -93,7 +93,7 @@ class MotionControlHandle : public controller_interface::ControllerInterface
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
controller_interface::InterfaceConfiguration state_interface_configuration() const override;

#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override;
#elif defined CARTESIAN_CONTROLLERS_FOXY
controller_interface::return_type update() override;
Expand Down
6 changes: 3 additions & 3 deletions cartesian_controller_handles/src/motion_control_handle.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ MotionControlHandle::on_deactivate(const rclcpp_lifecycle::State& previous_state
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
controller_interface::return_type MotionControlHandle::update(const rclcpp::Time& time,
const rclcpp::Duration& period)
#elif defined CARTESIAN_CONTROLLERS_FOXY
Expand Down Expand Up @@ -121,7 +121,7 @@ MotionControlHandle::state_interface_configuration() const
}


#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
MotionControlHandle::on_init()
{
Expand All @@ -141,7 +141,7 @@ controller_interface::return_type MotionControlHandle::init(const std::string& c
auto_declare<std::string>("end_effector_link", "");
auto_declare<std::vector<std::string> >("joints", std::vector<std::string>());

#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
#elif defined CARTESIAN_CONTROLLERS_FOXY
return controller_interface::return_type::OK;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ constexpr char HW_IF_DAMPING[] = "damping";
* controller_manager coordinated library.
*
*/
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
class Simulator : public hardware_interface::SystemInterface
#elif defined CARTESIAN_CONTROLLERS_FOXY
class Simulator : public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
Expand All @@ -83,7 +83,7 @@ class Simulator : public hardware_interface::BaseInterface<hardware_interface::S

RCLCPP_SHARED_PTR_DEFINITIONS(Simulator)

#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
#elif defined CARTESIAN_CONTROLLERS_FOXY
return_type configure(const hardware_interface::HardwareInfo& info) override;
Expand All @@ -97,7 +97,7 @@ class Simulator : public hardware_interface::BaseInterface<hardware_interface::S
const std::vector<std::string>& stop_interfaces) override;


#if defined CARTESIAN_CONTROLLERS_HUMBLE
#if defined CARTESIAN_CONTROLLERS_HUMBLE || defined CARTESIAN_CONTROLLERS_IRON
return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;

Expand Down
Loading