This commit is contained in:
_Luc_ 2015-05-27 17:24:18 +02:00
commit 0b4782098c
4 changed files with 113 additions and 274 deletions

View file

@ -52,15 +52,12 @@ target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRAR
target_link_libraries(normal_estimator ${catkin_LIBRARIES}) target_link_libraries(normal_estimator ${catkin_LIBRARIES})
add_dependencies(normal_estimator hand_control_generate_messages_cpp) add_dependencies(normal_estimator hand_control_generate_messages_cpp)
add_executable(commande src/commande.cpp)
target_link_libraries(commande ${catkin_LIBRARIES})
add_executable(commande-new src/commande-new.cpp)
target_link_libraries(commande-new ${catkin_LIBRARIES})
add_executable(commande-new-1d src/commande-new-1d.cpp) add_executable(commande-new-1d src/commande-new-1d.cpp)
target_link_libraries(commande-new-1d ${catkin_LIBRARIES}) target_link_libraries(commande-new-1d ${catkin_LIBRARIES})
add_executable(commande-abs src/commande-abs.cpp)
target_link_libraries(commande-abs ${catkin_LIBRARIES})
#add dynamic reconfigure api #add dynamic reconfigure api
generate_dynamic_reconfigure_options( generate_dynamic_reconfigure_options(
cfg/Filtre.cfg cfg/Filtre.cfg

View file

@ -16,6 +16,7 @@ gen.add("z_minimal_deviation", double_t, 0, "Absolute vertical movement detectio
gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 1.5, 0.) gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 1.5, 0.)
gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 3000, 0) gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 3000, 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", 15., 0., 180.)
exit(gen.generate(PACKAGE, "hand_control", "Commande")) exit(gen.generate(PACKAGE, "hand_control", "Commande"))

View file

@ -3,6 +3,7 @@
#include <locale.h> #include <locale.h>
#include <limits> #include <limits>
#include <math.h> #include <math.h>
#include <assert.h>
#include <pcl_ros/point_cloud.h> #include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
@ -11,29 +12,28 @@
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <math.h> #include <math.h>
#include <dynamic_reconfigure/server.h>
#include <hand_control/CommandeConfig.h>
class Run class Run
{ {
private: private:
float xx, yy, dz; float xx, yy, zz, theta;
// xx > 0 : forward // xx < 0 : forward
// xx < 0 : backward // xx > 0 : backward
// yy > 0 : right // yy > 0 : right
// yy < 0 : left // yy < 0 : left
// dz > 0 : up // zz > 0 : up
// dz < 0 : down // zz < 0 : down
float plan_vel, z_vel; float plan_vel, z_vel, angle_vel, up_factor, neutral_z;
// to calculate dz
float z_current, z_previous;
ros::Time t_current, t_previous;
// conditions to publish a message
float max_curv; float max_curv;
float dz_min, x_dev_min, y_dev_min; float z_dev_min, x_dev_min, y_dev_min, th_dev_min;
uint64_t min_number; uint64_t min_number;
bool first_msg; bool first_msg;
@ -43,56 +43,60 @@ class Run
void publish() void publish()
{ {
geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist()); geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
if (fabs(dz) > dz_min)
if (fabs(zz) > z_dev_min)
{ {
mvt->linear.z = dz * z_vel; if (zz > 0)
mvt->linear.z = zz * z_vel * up_factor ;
else
mvt->linear.z = zz * z_vel;
} }
if (fabs(xx) > x_dev_min)
{ if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min)
mvt->linear.x = xx * plan_vel;
}
if (fabs(yy) > y_dev_min)
{ {
mvt->linear.y = yy * plan_vel; mvt->linear.y = yy * plan_vel;
} }
else if (fabs(xx) > x_dev_min)
{
mvt->linear.x = - xx * plan_vel;
}
if (fabs(theta) > th_dev_min) {
mvt->angular.z * angle_vel;
}
assert(mvt->linear.x == 0. || mvt->linear.y == 0.);
pub.publish(mvt); pub.publish(mvt);
ROS_INFO("cmd published"); ROS_INFO("cmd published");
}//end publish }//end publish
public: public:
Run(const ros::Publisher& cmd_publisher, const float& max_curvature, const float& plan_velocity, Run(const ros::Publisher& cmd_publisher, const float& max_curvature, const float& plan_velocity, const float& angle_velocity,
const float& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation, const float& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation,
const float& dz_minimal_difference, const int& min_points_number) : const float& z_minimal_deviation, const float& neutral_alt, const float& theta_minimal_deviation,
const int& min_points_number, const float& up_fact) :
pub(cmd_publisher), pub(cmd_publisher),
plan_vel(plan_velocity), plan_vel(plan_velocity),
angle_vel(angle_velocity);
max_curv(max_curvature), max_curv(max_curvature),
z_vel(z_velocity), z_vel(z_velocity),
xx(0), xx(0),
yy(0), yy(0),
dz(0), zz(0),
theta(0),
x_dev_min(x_minimal_deviation), x_dev_min(x_minimal_deviation),
y_dev_min(y_minimal_deviation), y_dev_min(y_minimal_deviation),
dz_min(dz_minimal_difference), z_dev_min(z_minimal_deviation),
neutral_z(neutral_alt),
first_msg(true), first_msg(true),
min_number(min_points_number){ min_number(min_points_number),
z_current = z_previous = std::numeric_limits<float>::signaling_NaN(); up_factor(up_fact)
t_previous.nsec = t_previous.sec =
t_previous.nsec = t_previous.sec = std::numeric_limits<uint32_t>::signaling_NaN();
}
void callback(const hand_control::Plan::ConstPtr& msg) void callback(const hand_control::Plan::ConstPtr& msg)
{ {
ROS_INFO("plan received"); ROS_INFO("plan received");
if (msg->curvature < max_curv && msg->number > min_number) if (msg->curvature < max_curv && msg->number > min_number)
{ {
t_current = msg->header.stamp;
z_current = msg->altitude;
if (!first_msg)
{
dz = (z_current - z_previous)/((t_current - t_previous).toSec());
ROS_INFO("dz = %f", dz);
}
if(msg->normal.z > 0) if(msg->normal.z > 0)
{ {
@ -105,10 +109,10 @@ class Run
xx = - msg->normal.y; xx = - msg->normal.y;
} }
t_previous = t_current; zz = msg->altitude - neutral_z;
z_previous = z_current;
z_current = std::numeric_limits<float>::signaling_NaN(); theta = msg->angle;
t_current.nsec = t_current.sec = std::numeric_limits<uint32_t>::signaling_NaN();
if (first_msg) if (first_msg)
{ {
first_msg = false; first_msg = false;
@ -116,11 +120,22 @@ class Run
} }
ROS_INFO("coords updated"); ROS_INFO("coords updated");
} else { } else {
xx = yy = dz = 0.; xx = yy = zz = 0.;
} }
publish(); publish();
}; };
void reconfigure(const hand_control::CommandeConfig& c, const uint32_t& level) {
max_curv = c.max_curvature;
x_dev_min = c.x_minimal_deviation;
y_dev_min = c.y_minimal_deviation;
z_dev_min = c.z_minimal_deviation;
th_dev_min = c.theta_minimal_deviation;
neutral_z = c.neutral_alt;
min_number = c.min_points_number;
up_factor = c.up_fact;
}
void run() void run()
{ {
ros::spin(); ros::spin();
@ -163,6 +178,16 @@ int main(int argc, char** argv)
ROS_INFO("z_vel : %f (default value)", z_vel); ROS_INFO("z_vel : %f (default value)", z_vel);
} }
double angle_vel(0);
if (node.getParam("angle_vel", angle_vel))
{
ROS_INFO("angle_vel : %f" , angle_vel);
} else {
node.setParam("angle_vel", 10);
node.getParam("angle_vel", angle_vel);
ROS_INFO("angle_vel : %f (default value)", angle_vel);
}
int min_number(0); int min_number(0);
if (node.getParam("min_number", min_number)) if (node.getParam("min_number", min_number))
{ {
@ -193,19 +218,55 @@ int main(int argc, char** argv)
ROS_INFO("y_dev_min : %f (default value)", y_dev_min); ROS_INFO("y_dev_min : %f (default value)", y_dev_min);
} }
double dz_dev_min(0); double z_dev_min(0);
if (node.getParam("dz_dev_min", dz_dev_min)) if (node.getParam("z_dev_min", z_dev_min))
{ {
ROS_INFO("dz_dev_min : %f" , dz_dev_min); ROS_INFO("z_dev_min : %f" , z_dev_min);
} else { } else {
node.setParam("dz_dev_min", 0.05); node.setParam("z_dev_min", 0.1);
node.getParam("dz_dev_min", dz_dev_min); node.getParam("z_dev_min", z_dev_min);
ROS_INFO("dz_dev_min : %f (default value)", dz_dev_min); ROS_INFO("z_dev_min : %f (default value)", z_dev_min);
}
double th_dev_min(0);
if (node.getParam("th_dev_min", th_dev_min))
{
ROS_INFO("th_dev_min : %f" , th_dev_min);
} else {
node.setParam("th_dev_min", 15);
node.getParam("th_dev_min", th_dev_min);
ROS_INFO("th_dev_min : %f (default value)", th_dev_min);
}
double neutral_z(0);
if (node.getParam("neutral_z", neutral_z))
{
ROS_INFO("neutral_z : %f" , neutral_z);
} else {
node.setParam("neutral_z", 1.5);
node.getParam("neutral_z", neutral_z);
ROS_INFO("neutral_z : %f (default value)", neutral_z);
}
double up_fact(0);
if (node.getParam("up_fact", up_fact))
{
ROS_INFO("up_fact : %f" , up_fact);
} else {
node.setParam("up_fact", 1.5);
node.getParam("up_fact", up_fact);
ROS_INFO("up_fact : %f (default value)", up_fact);
} }
ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1); ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
Run run(cmd_pub, max_curv, plan_vel, z_vel, x_dev_min, y_dev_min, dz_dev_min, min_number); Run run(cmd_pub, max_curv, plan_vel, z_vel, angle_vel, x_dev_min, y_dev_min, z_dev_min, th_dev_min, neutral_z, min_number, up_fact);
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run); ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
dynamic_reconfigure::Server<hand_control::CommandeConfig> server;
dynamic_reconfigure::Server<hand_control::CommandeConfig>::CallbackType f;
f = boost::bind(&Run::reconfigure, &run, _1, _2);
server.setCallback(f);
run.run(); run.run();
return 0; return 0;
} }

View file

@ -1,220 +0,0 @@
#include <ros/ros.h>
#include <ros/time.h>
#include <locale.h>
#include <limits>
#include <math.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>
class Run
{
private:
ros::Rate loop_rate;
float xx, yy, dz;
// xx > 0 : forward
// xx < 0 : backward
// yy > 0 : right
// yy < 0 : left
// dz > 0 : up
// dz < 0 : down
float plan_vel, z_vel;
// to calculate dz
float z_current, z_previous;
ros::Time t_current, t_previous;
// conditions to publish a message
float max_curv;
float dz_min, x_dev_min, y_dev_min;
uint64_t min_number;
bool first_msg;
ros::Publisher pub;
void publish()
{
geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
if (fabs(dz) > dz_min)
{
mvt->linear.z = dz * z_vel;
ROS_INFO("z changed : %f, z_vel %f", mvt->linear.z, z_vel);
}
if (fabs(xx) > x_dev_min)
{
mvt->linear.x = xx * plan_vel;
}
if (fabs(yy) > y_dev_min)
{
mvt->linear.y = yy * plan_vel;
}
pub.publish(mvt);
ROS_INFO("cmd published");
}//end publish
public:
Run(const ros::Publisher& cmd_publisher, const float& max_curvature, const float& plan_velocity,
const float& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation,
const float& dz_minimal_difference, const int& min_points_number) :
pub(cmd_publisher),
plan_vel(plan_velocity),
max_curv(max_curvature),
z_vel(z_velocity),
xx(0),
yy(0),
dz(0),
x_dev_min(x_minimal_deviation),
y_dev_min(y_minimal_deviation),
dz_min(dz_minimal_difference),
first_msg(true),
loop_rate(30),
min_number(min_points_number){
z_current = z_previous = std::numeric_limits<float>::signaling_NaN();
t_previous.nsec = t_previous.sec =
t_previous.nsec = t_previous.sec = std::numeric_limits<uint32_t>::signaling_NaN();
}
void callback(const hand_control::Plan::ConstPtr& msg)
{
ROS_INFO("plan received");
if (msg->curvature < max_curv && msg->number > min_number)
{
t_current = msg->header.stamp;
z_current = msg->altitude;
if (!first_msg)
{
dz = (z_current - z_previous)/((t_current - t_previous).toSec());
ROS_INFO("dz = %f", dz);
}
if(msg->normal.z > 0)
{
yy = msg->normal.x;
xx = msg->normal.y;
}
else
{
yy = - msg->normal.x;
xx = - msg->normal.y;
}
t_previous = t_current;
z_previous = z_current;
z_current = std::numeric_limits<float>::signaling_NaN();
t_current.nsec = t_current.sec = std::numeric_limits<uint32_t>::signaling_NaN();
if (first_msg)
{
first_msg = false;
ROS_INFO("first msg received");
}
ROS_INFO("coords updated");
} else {
xx = yy = dz = 0.;
}
};
void run()
{
while(ros::ok())
{
publish();
ros::spinOnce();
loop_rate.sleep();
}
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "commande");
ros::NodeHandle node("commande");
double max_curv(0);
if (node.getParam("max_curv", max_curv))
{
ROS_INFO("max_curv : %f" , max_curv);
} else {
node.setParam("max_curv", 0.08);
node.getParam("max_curv", max_curv);
ROS_INFO("max_curv : %f (default value)", max_curv);
}
double plan_vel(0);
if (node.getParam("plan_vel", plan_vel))
{
ROS_INFO("plan_vel : %f" , plan_vel);
} else {
node.setParam("plan_vel", 0.8);
node.getParam("plan_vel", plan_vel);
ROS_INFO("plan_vel : %f (default value)", plan_vel);
}
double z_vel(0);
if (node.getParam("z_vel", z_vel))
{
ROS_INFO("z_vel : %f" , z_vel);
} else {
node.setParam("z_vel", 0.8);
node.getParam("z_vel", z_vel);
ROS_INFO("z_vel : %f (default value)", z_vel);
}
int min_number(0);
if (node.getParam("min_number", min_number))
{
ROS_INFO("min_number : %d" , min_number);
} else {
node.setParam("min_number", 500);
node.getParam("min_number", min_number);
ROS_INFO("min_number : %d (default value)", min_number);
}
double x_dev_min(0);
if (node.getParam("x_dev_min", x_dev_min))
{
ROS_INFO("x_dev_min : %f" , x_dev_min);
} else {
node.setParam("x_dev_min", 0.05);
node.getParam("x_dev_min", x_dev_min);
ROS_INFO("x_dev_min : %f (default value)", x_dev_min);
}
double y_dev_min(0);
if (node.getParam("y_dev_min", y_dev_min))
{
ROS_INFO("y_dev_min : %f" , y_dev_min);
} else {
node.setParam("y_dev_min", 0.05);
node.getParam("y_dev_min", y_dev_min);
ROS_INFO("y_dev_min : %f (default value)", y_dev_min);
}
double dz_dev_min(0);
if (node.getParam("dz_dev_min", dz_dev_min))
{
ROS_INFO("dz_dev_min : %f" , dz_dev_min);
} else {
node.setParam("dz_dev_min", 0.05);
node.getParam("dz_dev_min", dz_dev_min);
ROS_INFO("dz_dev_min : %f (default value)", dz_dev_min);
}
ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
Run run(cmd_pub, max_curv, plan_vel, z_vel, x_dev_min, y_dev_min, dz_dev_min, min_number);
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
run.run();
return 0;
}