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

Pose controller dynamic reconfig [WIP] #120

Open
wants to merge 2 commits into
base: devel
Choose a base branch
from
Open

Pose controller dynamic reconfig [WIP] #120

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
5dfdfed
Dynamic reconfigure added to pose_controller ros side.
Marco-Andarcia Nov 16, 2016
eb9ab14
Additional locking, w_min bugfix
Jan 16, 2017
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 yocs_diff_drive_pose_controller/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
project(yocs_diff_drive_pose_controller)

find_package(catkin REQUIRED COMPONENTS ecl_threads
dynamic_reconfigure
geometry_msgs
nodelet
pluginlib
Expand All @@ -11,11 +12,13 @@ find_package(catkin REQUIRED COMPONENTS ecl_threads
tf
yocs_controllers
yocs_math_toolkit
yocs_msgs
)

catkin_package(INCLUDE_DIRS include
LIBRARIES yocs_diff_drive_pose_controller yocs_diff_drive_pose_controller_nodelet
CATKIN_DEPENDS ecl_threads
dynamic_reconfigure
geometry_msgs
nodelet
pluginlib
Expand All @@ -25,6 +28,7 @@ catkin_package(INCLUDE_DIRS include
tf
yocs_controllers
yocs_math_toolkit
yocs_msgs
)

include_directories(include ${catkin_INCLUDE_DIRS})
Expand Down
3 changes: 3 additions & 0 deletions ...ve_pose_controller/include/yocs_diff_drive_pose_controller/diff_drive_pose_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@

#include <string>
#include <yocs_controllers/default_controller.hpp>
#include <ecl/threads/mutex.hpp>

namespace yocs
{
Expand Down Expand Up @@ -171,6 +172,8 @@ class DiffDrivePoseController : public Controller
double orient_eps_;
/// Enable or disable ros messages.
bool verbose_;

ecl::Mutex controller_mutex_;
};

} /* end namespace */
Expand Down
10 changes: 10 additions & 0 deletions ...ose_controller/include/yocs_diff_drive_pose_controller/diff_drive_pose_controller_ros.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,14 @@
** Includes
*****************************************************************************/
#include <ros/ros.h>
#include <ecl/threads/mutex.hpp>
#include <std_msgs/Empty.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <tf/transform_listener.h>
#include <boost/shared_ptr.hpp>
#include <dynamic_reconfigure/server.h>
#include <yocs_msgs/PoseControllerConfig.h>

#include "yocs_diff_drive_pose_controller/diff_drive_pose_controller.hpp"

Expand Down Expand Up @@ -132,6 +136,8 @@ class DiffDrivePoseControllerROS : public DiffDrivePoseController
*/
void disableCB(const std_msgs::EmptyConstPtr msg);

void reconfigCB(yocs_msgs::PoseControllerConfig &config, uint32_t level);

// basics
ros::NodeHandle nh_;
std::string name_;
Expand All @@ -156,6 +162,10 @@ class DiffDrivePoseControllerROS : public DiffDrivePoseController
std::string base_frame_name_;
/// frame name of the goal (pose)
std::string goal_frame_name_;

///dynamic reconfigure server
boost::shared_ptr<dynamic_reconfigure::Server<yocs_msgs::PoseControllerConfig> > reconfig_server_;

};

} // namespace yocs
Expand Down
9 changes: 7 additions & 2 deletions yocs_diff_drive_pose_controller/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,10 +12,11 @@
<url type="website">http://ros.org/wiki/yocs_diff_drive_pose_controller</url>
<url type="repository">https://github.com/yujinrobot/yujin_ocs</url>
<url type="bugtracker">https://github.com/yujinrobot/yujin_ocs/issues</url>

<buildtool_depend>catkin</buildtool_depend>

<build_depend>ecl_threads</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>pluginlib</build_depend>
Expand All @@ -25,8 +26,11 @@
<build_depend>tf</build_depend>
<build_depend>yocs_controllers</build_depend>
<build_depend>yocs_math_toolkit</build_depend>
<build_depend>yocs_msgs</build_depend>


<run_depend>ecl_threads</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>pluginlib</run_depend>
Expand All @@ -36,7 +40,8 @@
<run_depend>tf</run_depend>
<run_depend>yocs_controllers</run_depend>
<run_depend>yocs_math_toolkit</run_depend>

<run_depend>yocs_msgs</run_depend>

<export>
<nodelet plugin="${prefix}/plugins/nodelets.xml"/>
</export>
Expand Down
5 changes: 4 additions & 1 deletion yocs_diff_drive_pose_controller/src/diff_drive_pose_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,12 @@ void DiffDrivePoseController::setInput(double distance_to_goal, double delta, do

void DiffDrivePoseController::setCurrentLimits(double v_min, double w_min, double v_max, double w_max)
{
controller_mutex_.lock();
v_min_ = v_min;
w_min_ = w_min;
v_max_ = v_max;
w_max_ = w_max;
controller_mutex_.unlock();
}

bool DiffDrivePoseController::step()
Expand All @@ -62,6 +64,7 @@ bool DiffDrivePoseController::step()

void DiffDrivePoseController::calculateControls()
{
controller_mutex_.lock();
double angle_diff = mtk::wrapAngle(theta_ - delta_);

if (r_ > dist_thres_)
Expand Down Expand Up @@ -97,6 +100,7 @@ void DiffDrivePoseController::calculateControls()
}
}
}
controller_mutex_.unlock();
}

void DiffDrivePoseController::controlPose()
Expand Down Expand Up @@ -126,7 +130,6 @@ void DiffDrivePoseController::controlPose()

void DiffDrivePoseController::controlOrientation(double angle_difference)
{

w_ = orientation_gain_ * (angle_difference);

//enforce limits on angular velocity
Expand Down
33 changes: 33 additions & 0 deletions yocs_diff_drive_pose_controller/src/diff_drive_pose_controller_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,17 @@ bool DiffDrivePoseControllerROS::init()
"base_frame_name = " << base_frame_name_ <<", goal_frame_name = " << goal_frame_name_ << " [" << name_ <<"]");
ROS_DEBUG_STREAM(
"v_max = " << v_max_ <<", k_1 = " << k_1_ << ", k_2 = " << k_2_ << ", beta = " << beta_ << ", lambda = " << lambda_ << ", dist_thres = " << dist_thres_ << ", orient_thres = " << orient_thres_ <<" [" << name_ <<"]");

reconfig_server_ = boost::shared_ptr<dynamic_reconfigure::Server<yocs_msgs::PoseControllerConfig> >(
new dynamic_reconfigure::Server<yocs_msgs::PoseControllerConfig>(nh_));

///dynamic reconfigure server callback type
dynamic_reconfigure::Server<yocs_msgs::PoseControllerConfig>::CallbackType reconfig_callback_func;

reconfig_callback_func = boost::bind(&DiffDrivePoseControllerROS::reconfigCB, this, _1, _2);

reconfig_server_->setCallback(reconfig_callback_func);

return true;
}

Expand Down Expand Up @@ -214,9 +225,12 @@ void DiffDrivePoseControllerROS::setControlOutput()

void DiffDrivePoseControllerROS::controlMaxVelCB(const std_msgs::Float32ConstPtr msg)
{
controller_mutex_.lock();
v_max_ = msg->data;

//v_min_ = -v_max_; //if we also want to enable driving backwards
ROS_INFO_STREAM("Maximum linear control velocity has been set to " << v_max_ << ". [" << name_ << "]");
controller_mutex_.unlock();
}

void DiffDrivePoseControllerROS::enableCB(const std_msgs::StringConstPtr msg)
Expand Down Expand Up @@ -248,4 +262,23 @@ void DiffDrivePoseControllerROS::disableCB(const std_msgs::EmptyConstPtr msg)
}
}

void DiffDrivePoseControllerROS::reconfigCB(yocs_msgs::PoseControllerConfig &config, uint32_t level)
{
controller_mutex_.lock();
v_min_movement_ = config.v_min;
v_min_ = v_min_movement_;
v_max_ = config.v_max;
w_max_ = config.w_max;
w_min_ = -w_max_;
k_1_ = config.k_1;
k_2_ = config.k_2;
beta_ = config.beta;
lambda_ = config.lambda;
dist_thres_ = config.dist_thres;
orient_thres_ = config.orient_thres;
dist_eps_ = config.dist_eps;
orient_eps_ = config.orient_eps;
controller_mutex_.unlock();
}

} // namespace yocs