Skip to content

Commit

Permalink
Merge branch 'ros-controls:master' into gpio_controllers
Browse files Browse the repository at this point in the history
  • Loading branch information
mcbed authored Nov 16, 2022
2 parents db926bd + 39fce89 commit 4b9f37b
Show file tree
Hide file tree
Showing 4 changed files with 25 additions and 19 deletions.
14 changes: 14 additions & 0 deletions diff_drive_controller/src/diff_drive_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -471,6 +471,20 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
std::make_shared<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>(
odometry_publisher_);

std::string controller_namespace = std::string(get_node()->get_namespace());

if (controller_namespace == "/")
{
controller_namespace = "";
}
else
{
controller_namespace = controller_namespace + "/";
}

odom_params_.odom_frame_id = controller_namespace + odom_params_.odom_frame_id;
odom_params_.base_frame_id = controller_namespace + odom_params_.base_frame_id;

auto & odometry_message = realtime_odometry_publisher_->msg_;
odometry_message.header.frame_id = odom_params_.odom_frame_id;
odometry_message.child_frame_id = odom_params_.base_frame_id;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,6 @@
#include <memory>
#include <stdexcept>
#include <string>
// TODO(JafarAbdi): Remove experimental once the default standard is C++17
#include "experimental/optional"

// ROS
#include "rclcpp/rclcpp.hpp"
Expand Down Expand Up @@ -124,11 +122,11 @@ class GripperActionController : public controller_interface::ControllerInterface

bool verbose_ = false; ///< Hard coded verbose flag to help in debugging
std::string name_; ///< Controller name.
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_position_command_interface_;
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
joint_position_state_interface_;
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
joint_velocity_state_interface_;

std::shared_ptr<ParamListener> param_listener_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -306,9 +306,9 @@ template <const char * HardwareInterface>
controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_deactivate(
const rclcpp_lifecycle::State &)
{
joint_position_command_interface_ = std::experimental::nullopt;
joint_position_state_interface_ = std::experimental::nullopt;
joint_velocity_state_interface_ = std::experimental::nullopt;
joint_position_command_interface_ = std::nullopt;
joint_position_state_interface_ = std::nullopt;
joint_velocity_state_interface_ = std::nullopt;
release_interfaces();
return controller_interface::CallbackReturn::SUCCESS;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,6 @@
#include <memory>
#include <string>
#include <vector>
// TODO(JafarAbdi): Remove experimental once the default standard is C++17
#include "experimental/optional"

#include "control_toolbox/pid.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
Expand All @@ -44,7 +42,7 @@ class HardwareInterfaceAdapter
{
public:
bool init(
std::experimental::optional<
std::optional<
std::reference_wrapper<hardware_interface::LoanedCommandInterface>> /* joint_handle */,
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & /* node */)
{
Expand All @@ -71,8 +69,7 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_POSITION>
{
public:
bool init(
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_handle,
std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle,
const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */)
{
joint_handle_ = joint_handle;
Expand All @@ -92,8 +89,7 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_POSITION>
}

private:
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_handle_;
std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle_;
};

/**
Expand Down Expand Up @@ -130,8 +126,7 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
}

bool init(
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_handle,
std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle,
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
{
joint_handle_ = joint_handle;
Expand Down Expand Up @@ -182,8 +177,7 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
private:
using PidPtr = std::shared_ptr<control_toolbox::Pid>;
PidPtr pid_;
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_handle_;
std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle_;
std::chrono::steady_clock::time_point last_update_time_;
};

Expand Down

0 comments on commit 4b9f37b

Please sign in to comment.