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.

Sign up for GitHub

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

Jump to bottom

added support for tf_prefix #3

Merged
merged 3 commits into from
Nov 30, 2023
Merged

added support for tf_prefix #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
cc6da9e
added support for tf_prefix
delihus Nov 13, 2023
62c383d
removed whitespaces
delihus Nov 14, 2023
7d18f23
changed _ to /
delihus Nov 21, 2023
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
2 changes: 2 additions & 0 deletions mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,8 @@ class MecanumDriveController : public controller_interface::ControllerInterface
bool open_loop = false;
bool position_feedback = true;
bool enable_odom_tf = true;
bool tf_frame_prefix_enable = false;
std::string tf_frame_prefix = "";
std::string base_frame_id = "base_link";
std::string odom_frame_id = "odom";
std::array<double, 6> pose_covariance_diagonal;
Expand Down
37 changes: 34 additions & 3 deletions mecanum_drive_controller/src/mecanum_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_init()
auto_declare<bool>("open_loop", odom_params_.open_loop);
auto_declare<bool>("position_feedback", odom_params_.position_feedback);
auto_declare<bool>("enable_odom_tf", odom_params_.enable_odom_tf);
auto_declare<bool>("tf_frame_prefix_enable", odom_params_.tf_frame_prefix_enable);
auto_declare<std::string>("tf_frame_prefix", odom_params_.tf_frame_prefix);

auto_declare<double>("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0);
publish_limited_velocity_ = auto_declare<bool>("publish_limited_velocity", publish_limited_velocity_);
Expand Down Expand Up @@ -315,6 +317,31 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(const
auto logger = get_node()->get_logger();

// update parameters
odom_params_.tf_frame_prefix_enable = get_node()->get_parameter("tf_frame_prefix_enable").as_bool();
odom_params_.tf_frame_prefix = get_node()->get_parameter("tf_frame_prefix").as_string();
odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string();
odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string();
std::string tf_prefix;
if(odom_params_.tf_frame_prefix_enable){
if (odom_params_.tf_frame_prefix != "")
{
tf_prefix = odom_params_.tf_frame_prefix;
}
else
{
tf_prefix = std::string(get_node()->get_namespace());
}

if(tf_prefix == "/")
{
tf_prefix = "";
}
else
{
tf_prefix = tf_prefix + "/";
}
Comment on lines +335 to +342
Copy link
Contributor

@rafal-gorecki rafal-gorecki Nov 30, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should be equal

Suggested change
if(tf_prefix == "/")
{
tf_prefix = "";
}
else
{
tf_prefix = tf_prefix + "/";
}
if(tf_prefix != "/")
{
tf_prefix = tf_prefix + "/";
}

}

front_left_wheel_name_ = get_node()->get_parameter("front_left_wheel_name").as_string();
front_right_wheel_name_ = get_node()->get_parameter("front_right_wheel_name").as_string();
rear_left_wheel_name_ = get_node()->get_parameter("rear_left_wheel_name").as_string();
Expand All @@ -327,6 +354,13 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(const
return controller_interface::CallbackReturn::ERROR;
}

front_left_wheel_name_ = tf_prefix + front_left_wheel_name_;
front_right_wheel_name_ = tf_prefix + front_right_wheel_name_;
rear_left_wheel_name_ = tf_prefix + rear_left_wheel_name_;
rear_right_wheel_name_ = tf_prefix + rear_right_wheel_name_;
odom_params_.odom_frame_id = tf_prefix + odom_params_.odom_frame_id;
odom_params_.base_frame_id = tf_prefix + odom_params_.base_frame_id;

wheel_params_.separation_x = get_node()->get_parameter("wheel_separation_x").as_double();
wheel_params_.separation_y = get_node()->get_parameter("wheel_separation_y").as_double();
wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double();
Expand All @@ -343,9 +377,6 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(const
odometry_.setWheelParams(wheel_separation_x, wheel_separation_y, wheel_radius);
odometry_.setVelocityRollingWindowSize(get_node()->get_parameter("velocity_rolling_window_size").as_int());

odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string();
odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string();

auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array();
std::copy(pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin());

Expand Down