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

Add dynamic reconfigure #6

Merged
merged 2 commits into from
Jul 10, 2015
Merged

Add dynamic reconfigure #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
055f305
Add dynamic reconfigure server
Jul 9, 2015
fb4b05e
Make dynamic reconfigure params RT-safe
Jul 10, 2015
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
4 changes: 4 additions & 0 deletions diff_drive_controller/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@ project(diff_drive_controller)
find_package(catkin REQUIRED
COMPONENTS
cmake_modules
dynamic_reconfigure
controller_interface
urdf
realtime_tools
Expand All @@ -17,6 +18,8 @@ find_package(sophus REQUIRED)

find_package(Ceres REQUIRED)

generate_dynamic_reconfigure_options(cfg/DiffDriveController.cfg)

catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME})
Expand All @@ -32,6 +35,7 @@ roslint_cpp()
add_library(${PROJECT_NAME} src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp)
# Note that the entry for ${Ceres_LIBRARIES} was removed as we only used headers from that package
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)

install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
Expand Down
16 changes: 16 additions & 0 deletions diff_drive_controller/cfg/DiffDriveController.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
#!/usr/bin/env python

PACKAGE = 'diff_drive_controller'

from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t

gen = ParameterGenerator()

gen.add("wheel_separation_multiplier", double_t, 0, "Wheel separation multiplier.", 1.0, 0.5, 1.5)
gen.add("left_wheel_radius_multiplier", double_t, 0, "Left wheel radius multiplier.", 1.0, 0.5, 1.5)
gen.add("right_wheel_radius_multiplier", double_t, 0, "Right wheel radius multiplier.", 1.0, 0.5, 1.5)

gen.add("k_l", double_t, 0, "Covariance model k_l multiplier.", 1.0, 0.0, 10.0)
gen.add("k_r", double_t, 0, "Covariance model k_r multiplier.", 1.0, 0.0, 10.0)

exit(gen.generate(PACKAGE, "diff_drive_controller", "DiffDriveController"))
35 changes: 35 additions & 0 deletions diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,11 +46,14 @@
#include <nav_msgs/Odometry.h>
#include <tf/tfMessage.h>

#include <dynamic_reconfigure/server.h>

#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>

#include <diff_drive_controller/odometry.h>
#include <diff_drive_controller/speed_limiter.h>
#include <diff_drive_controller/DiffDriveControllerConfig.h>

#include <vector>
#include <string>
Expand Down Expand Up @@ -136,6 +139,30 @@ namespace diff_drive_controller
Odometry odometry_;
geometry_msgs::TransformStamped odom_frame_;

/// Dynamic reconfigure server related:
typedef dynamic_reconfigure::Server<DiffDriveControllerConfig> ReconfigureServer;
boost::shared_ptr<ReconfigureServer> cfg_server_;

struct DynamicParams
{
double wheel_separation_multiplier;
double left_wheel_radius_multiplier;
double right_wheel_radius_multiplier;

double k_l;
double k_r;

DynamicParams()
: wheel_separation_multiplier(1.0)
, left_wheel_radius_multiplier(1.0)
, right_wheel_radius_multiplier(1.0)
, k_l(1.0)
, k_r(1.0)
{}
};
realtime_tools::RealtimeBuffer<DynamicParams> dynamic_params_;
DynamicParams dynamic_params_struct_;

/// Wheel separation, wrt the midpoint of the wheel width:
double wheel_separation_;

Expand Down Expand Up @@ -180,6 +207,14 @@ namespace diff_drive_controller
*/
void cmdVelCallback(const geometry_msgs::Twist& command);

/**
* \brief Reconfigure callback
* \param [in, out] config Input new desired configuration and
* output applied configuration
* \param [in] level Reconfigure level
*/
void reconfigureCallback(DiffDriveControllerConfig& config, uint32_t level);

/**
* \brief Get the wheel names from a wheel param
* \param [in] controller_nh Controller node handler
Expand Down
2 changes: 2 additions & 0 deletions diff_drive_controller/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
<buildtool_depend>catkin</buildtool_depend>

<build_depend>cmake_modules</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>controller_interface</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>realtime_tools</build_depend>
Expand All @@ -25,6 +26,7 @@
<build_depend>libceres-dev</build_depend>
<build_depend>roslint</build_depend>

<run_depend>dynamic_reconfigure</run_depend>
<run_depend>controller_interface</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>realtime_tools</run_depend>
Expand Down
78 changes: 73 additions & 5 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,7 @@ namespace diff_drive_controller
DiffDriveController::DiffDriveController()
: open_loop_(false)
, command_struct_()
, dynamic_params_struct_()
, wheel_separation_(0.0)
, wheel_radius_(0.0)
, wheel_separation_multiplier_(1.0)
Expand Down Expand Up @@ -279,8 +280,23 @@ namespace diff_drive_controller
"Measurement Covariance Model params : k_l " << k_l_
<< ", k_r " << k_r_);

dynamic_params_struct_.wheel_separation_multiplier = wheel_separation_multiplier_;

dynamic_params_struct_.left_wheel_radius_multiplier = left_wheel_radius_multiplier_;
dynamic_params_struct_.right_wheel_radius_multiplier = right_wheel_radius_multiplier_;

dynamic_params_struct_.k_l = k_l_;
dynamic_params_struct_.k_r = k_r_;

dynamic_params_.writeFromNonRT(dynamic_params_struct_);

setOdomPubFields(root_nh, controller_nh);

// Set dynamic reconfigure server callback:
cfg_server_.reset(new ReconfigureServer(controller_nh));
cfg_server_->setCallback(
boost::bind(&DiffDriveController::reconfigureCallback, this, _1, _2));

// Get the joint object to use in the realtime loop
for (int i = 0; i < wheel_joints_size_; ++i)
{
Expand All @@ -298,6 +314,38 @@ namespace diff_drive_controller

void DiffDriveController::update(const ros::Time& time, const ros::Duration& period)
{
// UPDATE DYNAMIC PARAMS
// Retreive dynamic params:
DynamicParams dynamic_params = *(dynamic_params_.readFromRT());
Copy link

Choose a reason for hiding this comment

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

This is going to update all these parameters on control cycle, which seems like a lot. Would it be safe to use a boolean flag across threads to notify of update?

Copy link
Member Author

Choose a reason for hiding this comment

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

The problem is that there's not write from RT methods supported: see this proposal.

I could add a comment with that link, and we can go back to this discussion when this goes upstream; this is fast enough anyway. What do you think?

Copy link
Member Author

Choose a reason for hiding this comment

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

Comment added. Merging...


// Update dynamic params:
//
// NOTE we cannot do the following because there's no writeFromRT method!
// see https://github.com/ros-controls/realtime_tools/issues/14
//
// if (dynamic_params.changed)
// {
// dynamic_params.changed = false;
// dynamic_params_.writeFromRT(dynamic_params);
//
// ...
// }
wheel_separation_multiplier_ = dynamic_params.wheel_separation_multiplier;
left_wheel_radius_multiplier_ = dynamic_params.left_wheel_radius_multiplier;
right_wheel_radius_multiplier_ = dynamic_params.right_wheel_radius_multiplier;

k_l_ = dynamic_params.k_l;
k_r_ = dynamic_params.k_r;

// Apply multipliers:
const double ws = wheel_separation_multiplier_ * wheel_separation_;
const double wrl = left_wheel_radius_multiplier_ * wheel_radius_;
const double wrr = right_wheel_radius_multiplier_ * wheel_radius_;

// Set the odometry parameters:
odometry_.setWheelParams(ws, wrl, wrr);
odometry_.setMeasCovarianceParams(k_l_, k_r_);

// COMPUTE AND PUBLISH ODOMETRY
if (open_loop_)
{
Expand Down Expand Up @@ -380,11 +428,6 @@ namespace diff_drive_controller
limiter_ang_.limit(curr_cmd.ang, last_cmd_.ang, cmd_dt);
last_cmd_ = curr_cmd;

// Apply multipliers:
const double ws = wheel_separation_multiplier_ * wheel_separation_;
const double wrl = left_wheel_radius_multiplier_ * wheel_radius_;
const double wrr = right_wheel_radius_multiplier_ * wheel_radius_;

// Compute wheels velocities:
const double vel_left = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/wrl;
const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/wrr;
Expand Down Expand Up @@ -442,6 +485,31 @@ namespace diff_drive_controller
}
}

void DiffDriveController::reconfigureCallback(
DiffDriveControllerConfig& config, uint32_t level)
{
dynamic_params_struct_.wheel_separation_multiplier = config.wheel_separation_multiplier;

dynamic_params_struct_.left_wheel_radius_multiplier = config.left_wheel_radius_multiplier;
dynamic_params_struct_.right_wheel_radius_multiplier = config.right_wheel_radius_multiplier;

dynamic_params_struct_.k_l = config.k_l;
dynamic_params_struct_.k_r = config.k_r;

dynamic_params_.writeFromNonRT(dynamic_params_struct_);

ROS_DEBUG_STREAM_NAMED(name_,
"Reconfigured Odometry params. "
<< "wheel separation: " << dynamic_params_struct_.wheel_separation_multiplier << ", "
<< "left wheel radius: " << dynamic_params_struct_.left_wheel_radius_multiplier << ", "
<< "right wheel radius: " << dynamic_params_struct_.left_wheel_radius_multiplier);

ROS_DEBUG_STREAM_NAMED(name_,
"Reconfigured Measurement Covariance Model params. "
<< "k_l: " << dynamic_params_struct_.k_l << ", "
<< "k_r: " << dynamic_params_struct_.k_r);
}

bool DiffDriveController::getWheelNames(ros::NodeHandle& controller_nh,
const std::string& wheel_param,
std::vector<std::string>& wheel_names)
Expand Down