Skip to content
Merged
Show file tree
Hide file tree
Changes from 3 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
7 changes: 6 additions & 1 deletion admittance_controller/test/test_admittance_controller.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -122,11 +122,16 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon
return success;
}

rclcpp::NodeOptions get_node_options() const override { return node_options_; }

void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; }

private:
rclcpp::WaitSet input_wrench_command_subscriber_wait_set_;
rclcpp::WaitSet input_pose_command_subscriber_wait_set_;
const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf;
const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf;
rclcpp::NodeOptions node_options_;
};

class AdmittanceControllerTest : public ::testing::Test
Expand Down Expand Up @@ -185,7 +190,7 @@ class AdmittanceControllerTest : public ::testing::Test
controller_interface::return_type SetUpControllerCommon(
const std::string & controller_name, const rclcpp::NodeOptions & options)
{
auto result = controller_->init(controller_name, "", 0, "", options);
auto result = controller_->init(controller_name, "", 0, "");

controller_->export_reference_interfaces();
assign_interfaces();
Expand Down
4 changes: 3 additions & 1 deletion doc/writing_new_controller.rst
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,9 @@ The following is a step-by-step guide to create source files, basic tests, and c
5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``on_init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``.
For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers <https://github.com/ros-controls/ros2_controllers>`_.

6. (optional) Often, controllers accept lists of joint names and interface names as parameters.
6. (Optional) The NodeOptions of the LifecycleNode can be personalized by overriding the default method ``get_node_options``.

7. (Optional) Often, controllers accept lists of joint names and interface names as parameters.
If so, you can add two protected string vectors to store those values.

4. **Adding definitions into source file (.cpp)**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,8 @@ class TestableJointTrajectoryController
return ret;
}

rclcpp::NodeOptions get_node_options() const override { return node_options_; }

/**
* @brief wait_for_trajectory block until a new JointTrajectory is received.
* Requires that the executor is not spinned elsewhere between the
Expand Down Expand Up @@ -157,6 +159,8 @@ class TestableJointTrajectoryController

double get_cmd_timeout() { return cmd_timeout_; }

void set_node_options(const rclcpp::NodeOptions & node_options) { node_options_ = node_options; }

trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; }
trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; }
trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; }
Expand All @@ -179,6 +183,7 @@ class TestableJointTrajectoryController
}

rclcpp::WaitSet joint_cmd_sub_wait_set_;
rclcpp::NodeOptions node_options_;
};

class TrajectoryControllerTest : public ::testing::Test
Expand Down Expand Up @@ -233,8 +238,9 @@ class TrajectoryControllerTest : public ::testing::Test
parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_));
parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end());
node_options.parameter_overrides(parameter_overrides);
traj_controller_->set_node_options(node_options);

return traj_controller_->init(controller_name_, "", 0, "", node_options);
return traj_controller_->init(controller_name_, "", 0, "");
}

void SetPidParameters(
Expand Down