Skip to content

Commit

Permalink
ROS2 control hwif and launch config update (#185)
Browse files Browse the repository at this point in the history 
* update get_state to get_lifecycle_state

* update controller spawning

* formatting

* remove controller config file params from the control_node
  • Loading branch information
@PetoAdam
PetoAdam authored Oct 4, 2024
1 parent 81a2d23 commit bd4ffa5
Show file tree
Hide file tree
Showing 4 changed files with 57 additions and 41 deletions.
33 changes: 19 additions & 14 deletions kuka_iiqka_eac_driver/launch/startup.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,9 +113,6 @@ def launch_setup(context, *args, **kwargs):
parameters=[
robot_description,
controller_config,
jtc_config,
jic_config,
ec_config,
{
"hardware_components_initial_state": {
"unconfigured": [tf_prefix + robot_model.perform(context)]
Expand Down Expand Up @@ -145,29 +142,37 @@ def launch_setup(context, *args, **kwargs):
)

# Spawn controllers
def controller_spawner(controller_names, activate=False):
def controller_spawner(controller_name, param_file=None, activate=False):
arg_list = [
controller_names,
controller_name,
"-c",
controller_manager_node,
"-n",
ns,
]

# Add param-file if it's provided
if param_file:
arg_list.extend(["--param-file", param_file])

if not activate:
arg_list.append("--inactive")

return Node(package="controller_manager", executable="spawner", arguments=arg_list)

controller_names = [
"joint_state_broadcaster",
"joint_trajectory_controller",
"joint_group_impedance_controller",
"effort_controller",
"control_mode_handler",
"event_broadcaster",
controllers = {
"joint_state_broadcaster": None,
"joint_trajectory_controller": jtc_config,
"joint_group_impedance_controller": jic_config,
"effort_controller": ec_config,
"control_mode_handler": None,
"event_broadcaster": None,
}

controller_spawners = [
controller_spawner(name, param_file) for name, param_file in controllers.items()
]

controller_spawners = [controller_spawner(name) for name in controller_names]

nodes_to_start = [
control_node,
robot_manager_node,
Expand Down
23 changes: 15 additions & 8 deletions kuka_kss_rsi_driver/launch/startup.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,6 @@ def launch_setup(context, *args, **kwargs):
parameters=[
robot_description,
controller_config,
jtc_config,
{
"hardware_components_initial_state": {
"unconfigured": [tf_prefix + robot_model.perform(context)]
Expand All @@ -128,24 +127,32 @@ def launch_setup(context, *args, **kwargs):
)

# Spawn controllers
def controller_spawner(controller_names, activate=False):
def controller_spawner(controller_name, param_file=None, activate=False):
arg_list = [
controller_names,
controller_name,
"-c",
controller_manager_node,
"-n",
ns,
]

# Add param-file if it's provided
if param_file:
arg_list.extend(["--param-file", param_file])

if not activate:
arg_list.append("--inactive")

return Node(package="controller_manager", executable="spawner", arguments=arg_list)

controller_names = [
"joint_state_broadcaster",
"joint_trajectory_controller",
]
controllers = {
"joint_state_broadcaster": None,
"joint_trajectory_controller": jtc_config,
}

controller_spawners = [controller_spawner(name) for name in controller_names]
controller_spawners = [
controller_spawner(name, param_file) for name, param_file in controllers.items()
]

nodes_to_start = [
control_node,
Expand Down
2 changes: 1 addition & 1 deletion kuka_kss_rsi_driver/src/hardware_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -168,7 +168,7 @@ return_type KukaRSIHardwareInterface::read(const rclcpp::Time &, const rclcpp::D
if (server_->recv(in_buffer_) == 0)
{
RCLCPP_ERROR(rclcpp::get_logger("KukaRSIHardwareInterface"), "No data received from robot");
this->on_deactivate(this->get_state());
this->on_deactivate(this->get_lifecycle_state());
return return_type::ERROR;
}
rsi_state_ = RSIState(in_buffer_);
Expand Down
40 changes: 22 additions & 18 deletions kuka_sunrise_fri_driver/launch/startup.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,10 +114,6 @@ def launch_setup(context, *args, **kwargs):
parameters=[
robot_description,
controller_config,
jtc_config,
jic_config,
ec_config,
etb_config,
{
"hardware_components_initial_state": {
"unconfigured": [tf_prefix + robot_model.perform(context)]
Expand Down Expand Up @@ -147,32 +143,40 @@ def launch_setup(context, *args, **kwargs):
)

# Spawn controllers
def controller_spawner(controller_names, activate=False):
def controller_spawner(controller_name, param_file=None, activate=False):
arg_list = [
controller_names,
controller_name,
"-c",
controller_manager_node,
"-n",
ns,
]

# Add param-file if it's provided
if param_file:
arg_list.extend(["--param-file", param_file])

if not activate:
arg_list.append("--inactive")

return Node(package="controller_manager", executable="spawner", arguments=arg_list)

controller_names = [
"joint_state_broadcaster",
"external_torque_broadcaster",
"joint_trajectory_controller",
"fri_configuration_controller",
"fri_state_broadcaster",
"joint_group_impedance_controller",
"effort_controller",
"control_mode_handler",
"event_broadcaster",
controllers = {
"joint_state_broadcaster": None,
"external_torque_broadcaster": etb_config,
"joint_trajectory_controller": jtc_config,
"fri_configuration_controller": None,
"fri_state_broadcaster": None,
"joint_group_impedance_controller": jic_config,
"effort_controller": ec_config,
"control_mode_handler": None,
"event_broadcaster": None,
}

controller_spawners = [
controller_spawner(name, param_file) for name, param_file in controllers.items()
]

controller_spawners = [controller_spawner(name) for name in controller_names]

nodes_to_start = [
control_node,
robot_manager_node,
Expand Down

0 comments on commit bd4ffa5

Please sign in to comment.