diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 6cbbbe06c6..ffc28767ae 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -31,6 +31,7 @@ joint_state_broadcaster * Added parameter ``publish_dynamic_joint_states`` to enable/disable publishing of dynamic joint states. (`#2064 `_) * Removed interfaces with other data types than double for publishing to ``dynamic_joint_states``. (`#2115 `_) * Parameter ``publish_dynamic_joint_states`` is now deprecated (default changed to ``false``). +* New parameters ``initial_value.position``, ``initial_value.velocity``, and ``initial_value.effort`` to set the initial values for interfaces omni_wheel_drive_controller ***************************** diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index b24152b0d8..2292654892 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -264,7 +264,23 @@ bool JointStateBroadcaster::init_joint_data() { interface_name = map_interface_to_joint_state_[interface_name]; } - name_if_value_mapping_[prefix_name][interface_name] = kUninitializedValue; + + if (interface_name == HW_IF_POSITION) + { + name_if_value_mapping_[prefix_name][interface_name] = params_.initial_value.position; + } + else if (interface_name == HW_IF_VELOCITY) + { + name_if_value_mapping_[prefix_name][interface_name] = params_.initial_value.velocity; + } + else if (interface_name == HW_IF_EFFORT) + { + name_if_value_mapping_[prefix_name][interface_name] = params_.initial_value.effort; + } + else + { + name_if_value_mapping_[prefix_name][interface_name] = kUninitializedValue; + } // filter state interfaces that have at least one of the joint_states fields, // the rest will be ignored for this message @@ -345,9 +361,9 @@ void JointStateBroadcaster::init_joint_state_msg() // default initialization for joint state message joint_state_msg_.header.frame_id = frame_id_; joint_state_msg_.name = joint_names_; - joint_state_msg_.position.resize(num_joints, kUninitializedValue); - joint_state_msg_.velocity.resize(num_joints, kUninitializedValue); - joint_state_msg_.effort.resize(num_joints, kUninitializedValue); + joint_state_msg_.position.resize(num_joints, params_.initial_value.position); + joint_state_msg_.velocity.resize(num_joints, params_.initial_value.velocity); + joint_state_msg_.effort.resize(num_joints, params_.initial_value.effort); // save joint state data auto get_address = @@ -361,16 +377,33 @@ void JointStateBroadcaster::init_joint_state_msg() } else { - return kUninitializedValue; + if (interface_name == HW_IF_POSITION) + { + return params_.initial_value.position; + } + else if (interface_name == HW_IF_VELOCITY) + { + return params_.initial_value.velocity; + } + else if (interface_name == HW_IF_EFFORT) + { + return params_.initial_value.effort; + } + else + { + return kUninitializedValue; + } } }; joint_states_data_.clear(); for (auto i = 0u; i < joint_names_.size(); ++i) { - joint_states_data_.push_back(JointStateData( - get_address(joint_names_[i], HW_IF_POSITION), get_address(joint_names_[i], HW_IF_VELOCITY), - get_address(joint_names_[i], HW_IF_EFFORT))); + joint_states_data_.push_back(JointStateData{ + get_address(joint_names_[i], HW_IF_POSITION), + get_address(joint_names_[i], HW_IF_VELOCITY), + get_address(joint_names_[i], HW_IF_EFFORT), + }); } } @@ -388,7 +421,7 @@ void JointStateBroadcaster::init_dynamic_joint_state_msg() for (const auto & interface_and_value : interfaces_and_values) { if_value.interface_names.emplace_back(interface_and_value.first); - if_value.values.emplace_back(kUninitializedValue); + if_value.values.emplace_back(std::numeric_limits::quiet_NaN()); } dynamic_joint_state_msg_.interface_values.emplace_back(if_value); } diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index 2993dddceb..9912cca3f8 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -30,6 +30,25 @@ joint_state_broadcaster: If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be published." } + initial_value: + position: { + type: double, + default_value: .nan, + read_only: true, + description: "Initial joint position value." + } + velocity: { + type: double, + default_value: .nan, + read_only: true, + description: "Initial joint velocity value." + } + effort: { + type: double, + default_value: .nan, + read_only: true, + description: "Initial joint effort value." + } map_interface_to_joint_state: position: { type: string,