adds commander_atan node to replace commander node

This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-28 21:53:07 +02:00
parent 1b601b9f22
commit b19ea6355c
5 changed files with 178 additions and 7 deletions

View file

@ -53,6 +53,10 @@ target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRAR
target_link_libraries(estimator ${catkin_LIBRARIES}) target_link_libraries(estimator ${catkin_LIBRARIES})
add_dependencies(estimator hand_control_generate_messages_cpp) add_dependencies(estimator hand_control_generate_messages_cpp)
add_executable(commander_atan src/commander_atan.cpp)
target_link_libraries(commander_atan ${catkin_LIBRARIES})
add_dependencies(commander_atan hand_control_generate_messages_cpp)
add_executable(commander src/commander.cpp) add_executable(commander src/commander.cpp)
target_link_libraries(commander ${catkin_LIBRARIES}) target_link_libraries(commander ${catkin_LIBRARIES})
add_dependencies(commander hand_control_generate_messages_cpp) add_dependencies(commander hand_control_generate_messages_cpp)
@ -62,11 +66,13 @@ generate_dynamic_reconfigure_options(
cfg/Filter.cfg cfg/Filter.cfg
cfg/Commander.cfg cfg/Commander.cfg
cfg/Estimator.cfg cfg/Estimator.cfg
cfg/Commander_atan.cfg
) )
add_dependencies(filter ${PROJECT_NAME}_gencfg) add_dependencies(filter ${PROJECT_NAME}_gencfg)
add_dependencies(commander ${PROJECT_NAME}_gencfg) add_dependencies(commander ${PROJECT_NAME}_gencfg)
add_dependencies(estimator ${PROJECT_NAME}_gencfg) add_dependencies(estimator ${PROJECT_NAME}_gencfg)
add_dependencies(commander_atan ${PROJECT_NAME}_gencfg)
catkin_package( catkin_package(
CATKIN_DEPENDS message_runtime CATKIN_DEPENDS message_runtime

View file

@ -3,14 +3,14 @@ PACKAGE = "hand_control"
from dynamic_reconfigure.parameter_generator_catkin import * from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator() gen = ParameterGenerator()
gen.add("max_curvature", double_t, 0, "Maximum curvature of the estimated plane", 0.5, 0., 1.) gen.add("max_curvature", double_t, 0, "Maximum curvature of the estimated plane", 0.5, 0., 1.)
gen.add("x_minimal_deviation", double_t, 0, "Absolute horizontal movement detection treshold", 0.05, 0.) gen.add("x_minimal_deviation", double_t, 0, "Absolute horizontal movement detection treshold", 0.1, 0.)
gen.add("y_minimal_deviation", double_t, 0, "Absolute lateral movement detection treshold", 0.05, 0.) gen.add("y_minimal_deviation", double_t, 0, "Absolute lateral movement detection treshold", 0.1, 0.)
gen.add("z_minimal_deviation", double_t, 0, "Absolute vertical movement detection treshold", 0.1, 0.) gen.add("z_minimal_deviation", double_t, 0, "Absolute vertical movement detection treshold", 0.1, 0.)
gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 0.8, 0.) gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 0.8, 0.)
gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 2000, 0) gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 1000, 0)
gen.add("up_fact", double_t, 0, "Upward command amplification factor", 1.5, 1) gen.add("up_fact", double_t, 0, "Upward command amplification factor", 1.5, 1)
gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 5., 0., 45.) gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 15., 0., 45.)
gen.add("angle_vel", double_t, 0, "Angular velocity", 0.002, 0., 10.) gen.add("angle_vel", double_t, 0, "Angular velocity", 0.01, 0., 10.)
gen.add("plan_vel", double_t, 0, "Translation velocity", 2, 0., 10.) gen.add("plan_vel", double_t, 0, "Translation velocity", 0.5, 0., 10.)
gen.add("z_vel", double_t, 0, "Vertical translation velocity", 2, 0., 10.) gen.add("z_vel", double_t, 0, "Vertical translation velocity", 2., 0., 10.)
exit(gen.generate(PACKAGE, "hand_control", "Commander")) exit(gen.generate(PACKAGE, "hand_control", "Commander"))

16
cfg/Commander_atan.cfg Executable file
View file

@ -0,0 +1,16 @@
#!/usr/bin/env python
PACKAGE = "hand_control"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("max_curvature", double_t, 0, "Maximum curvature of the estimated plane", 0.5, 0., 1.)
gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 0.8, 0.)
gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 1000, 0)
gen.add("p_x", double_t, 0, "x gradient in 0", 0.5, 0.)
gen.add("p_y", double_t, 0, "y gradient in 0", 0.5, 0.)
gen.add("p_z", double_t, 0, "z gradient in 0", 0.5, 0.)
gen.add("p_th", double_t, 0, "th gradient in 0", 0.01, 0.)
gen.add("x_min", double_t, 0, "min x coord to be published", 0.1, 0., 1.)
gen.add("y_min", double_t, 0, "min y coord to be published", 0.1, 0., 1.)
gen.add("z_min", double_t, 0, "min z coord to be published", 0.1, 0., 1.)
gen.add("th_min", double_t, 0, "min th coord to be published", 0.1, 0., 1.)
exit(gen.generate(PACKAGE, "hand_control", "Commander_atan"))

140
src/commander_atan.cpp Normal file
View file

@ -0,0 +1,140 @@
#include <ros/ros.h>
#include <ros/time.h>
#include <locale.h>
#include <limits>
#include <math.h>
#include <assert.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <hand_control/Plan.h>
#include <geometry_msgs/Twist.h>
#include <math.h>
#include <dynamic_reconfigure/server.h>
#include <hand_control/Commander_atanConfig.h>
#include "data.h"
class Run
{
private:
class Function
{
private:
float t;
static const float demi_pi;
public:
float operator()(float arg)
{
return atan(t * arg) / demi_pi;
}
Function() : t(demi_pi){}
void set_t(float tt)
{
t = tt;
}
void set_p(float pp)
// pp : gradient in 0
{
t = pp*demi_pi;
}
};
Data<Function> f;
Data<float> min;
float neutral_z;
float max_curv;
uint64_t min_number;
ros::Publisher pub;
public:
Run(const ros::Publisher& cmd_publisher) :
pub(cmd_publisher) {
f.x = f.y = f.z = f.th = Function();
min.x = min.y = min.z = min.th = 0.1;
}
void callback(const hand_control::Plan::ConstPtr& msg)
{
ROS_INFO("plan received");
Data<float> in, out;
in.x = in.y = in.z = in.th = 0.;
out.x = out.y = out.z = out.th = 0;
if (msg->curvature < max_curv && msg->number > min_number)
{
if(msg->normal.z > 0)
{
in.y = msg->normal.x;
in.x = msg->normal.y;
} else {
in.y = - msg->normal.x;
in.x = - msg->normal.y;
}
in.z = msg->altitude - neutral_z;
in.th = msg->angle;
}
geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
if (fabs(in.y) > fabs(in.x))
{
out.y = f.y(in.y);
if (out.y > min.y)
mvt->linear.y = out.y;
} else { {
out.x = f.x(in.x);
if (out.x > min.x)
mvt->linear.x = out.x;
}
out.z = f.z(in.z);
if (out.z > min.z)
mvt->linear.z = out.z;
out.th = f.th(in.th);
if (out.th > min.th)
mvt->angular.z = out.th;
pub.publish(mvt);
ROS_INFO("cmd published");
}
}
void reconfigure(const hand_control::Commander_atanConfig& c, const uint32_t& level)
{
max_curv = c.max_curvature;
neutral_z = c.neutral_alt;
min_number = c.min_points_number;
f.x.set_p(c.p_x);
f.y.set_p(c.p_y);
f.z.set_p(c.p_z);
f.th.set_p(c.p_th);
min.x = c.x_min;
min.y = c.y_min;
min.z = c.z_min;
min.th = c.th_min;
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "commander_atan");
ros::NodeHandle node("commander_atan");
ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
Run run(cmd_pub);
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
dynamic_reconfigure::Server<hand_control::Commander_atanConfig> server;
dynamic_reconfigure::Server<hand_control::Commander_atanConfig>::CallbackType f;
f = boost::bind(&Run::reconfigure, &run, _1, _2);
server.setCallback(f);
ROS_INFO("start spinning");
ros::spin();
return 0;
}
const float Run::Function::demi_pi = 2*atan(1.);

9
src/data.h Normal file
View file

@ -0,0 +1,9 @@
#ifndef DATA_H
#define DATA_H
template <typename T>
struct Data
{
public:
T x, y, z, th;
};
#endif