From 5c3bac74732c0ffdfe3855c70f88df504e0d5d87 Mon Sep 17 00:00:00 2001 From: Paul Janin Date: Wed, 27 May 2015 16:37:13 +0200 Subject: [PATCH 1/4] Added support for vertical rotation in "commande" node --- cfg/Commande.cfg | 1 + 1 file changed, 1 insertion(+) diff --git a/cfg/Commande.cfg b/cfg/Commande.cfg index 69e0aa2..d24b414 100755 --- a/cfg/Commande.cfg +++ b/cfg/Commande.cfg @@ -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("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("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 15., 0., 180.) exit(gen.generate(PACKAGE, "hand_control", "Commande")) From 444a8d26a7304a389daac20f21cc902c4e5d1c04 Mon Sep 17 00:00:00 2001 From: Paul Janin Date: Wed, 27 May 2015 16:40:19 +0200 Subject: [PATCH 2/4] rm obsolete versions of "commande" source code, please use file "commande-abs.cpp" --- src/commande-new.cpp | 211 ----------------------------------------- src/commande.cpp | 220 ------------------------------------------- 2 files changed, 431 deletions(-) delete mode 100644 src/commande-new.cpp delete mode 100644 src/commande.cpp diff --git a/src/commande-new.cpp b/src/commande-new.cpp deleted file mode 100644 index f84b743..0000000 --- a/src/commande-new.cpp +++ /dev/null @@ -1,211 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -class Run -{ - private: - 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; - } - 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), - min_number(min_points_number){ - z_current = z_previous = std::numeric_limits::signaling_NaN(); - t_previous.nsec = t_previous.sec = - t_previous.nsec = t_previous.sec = std::numeric_limits::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::signaling_NaN(); - t_current.nsec = t_current.sec = std::numeric_limits::signaling_NaN(); - if (first_msg) - { - first_msg = false; - ROS_INFO("first msg received"); - } - ROS_INFO("coords updated"); - } else { - xx = yy = dz = 0.; - } - publish(); - }; - - void run() - { - ros::spin(); - } - -}; - -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("/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("input", 1, &Run::callback, &run); - run.run(); - return 0; -} diff --git a/src/commande.cpp b/src/commande.cpp deleted file mode 100644 index 4c78655..0000000 --- a/src/commande.cpp +++ /dev/null @@ -1,220 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -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::signaling_NaN(); - t_previous.nsec = t_previous.sec = - t_previous.nsec = t_previous.sec = std::numeric_limits::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::signaling_NaN(); - t_current.nsec = t_current.sec = std::numeric_limits::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("/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("input", 1, &Run::callback, &run); - run.run(); - return 0; -} From 5da4126da991dfd5dfc0fc4781d21d901ad9c31c Mon Sep 17 00:00:00 2001 From: Paul Janin Date: Wed, 27 May 2015 16:49:47 +0200 Subject: [PATCH 3/4] Updated CMakeLists.txt --- CMakeLists.txt | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 387a761..a89c12e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,15 +52,12 @@ target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRAR target_link_libraries(normal_estimator ${catkin_LIBRARIES}) 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) 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 generate_dynamic_reconfigure_options( cfg/Filtre.cfg From fb2c88144f679ce50c174dc0474bbcacad14b297 Mon Sep 17 00:00:00 2001 From: Paul Janin Date: Wed, 27 May 2015 16:52:09 +0200 Subject: [PATCH 4/4] Finally added "commande-abs.cpp" to git repository --- src/commande-abs.cpp | 272 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 272 insertions(+) create mode 100644 src/commande-abs.cpp diff --git a/src/commande-abs.cpp b/src/commande-abs.cpp new file mode 100644 index 0000000..cc6cd61 --- /dev/null +++ b/src/commande-abs.cpp @@ -0,0 +1,272 @@ +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + + +class Run +{ + private: + float xx, yy, zz, theta; + + // xx < 0 : forward + // xx > 0 : backward + + // yy > 0 : right + // yy < 0 : left + + // zz > 0 : up + // zz < 0 : down + + float plan_vel, z_vel, angle_vel, up_factor, neutral_z; + + float max_curv; + float z_dev_min, x_dev_min, y_dev_min, th_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(zz) > z_dev_min) + { + if (zz > 0) + mvt->linear.z = zz * z_vel * up_factor ; + else + mvt->linear.z = zz * z_vel; + } + + if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min) + { + 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); + ROS_INFO("cmd published"); + }//end publish + + public: + 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_minimal_deviation, const float& neutral_alt, const float& theta_minimal_deviation, + const int& min_points_number, const float& up_fact) : + pub(cmd_publisher), + plan_vel(plan_velocity), + angle_vel(angle_velocity); + max_curv(max_curvature), + z_vel(z_velocity), + xx(0), + yy(0), + zz(0), + theta(0), + x_dev_min(x_minimal_deviation), + y_dev_min(y_minimal_deviation), + z_dev_min(z_minimal_deviation), + neutral_z(neutral_alt), + first_msg(true), + min_number(min_points_number), + up_factor(up_fact) + + void callback(const hand_control::Plan::ConstPtr& msg) + { + ROS_INFO("plan received"); + if (msg->curvature < max_curv && msg->number > min_number) + { + + if(msg->normal.z > 0) + { + yy = msg->normal.x; + xx = msg->normal.y; + } + else + { + yy = - msg->normal.x; + xx = - msg->normal.y; + } + + zz = msg->altitude - neutral_z; + + theta = msg->angle; + + if (first_msg) + { + first_msg = false; + ROS_INFO("first msg received"); + } + ROS_INFO("coords updated"); + } else { + xx = yy = zz = 0.; + } + 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() + { + ros::spin(); + } + +}; + +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); + } + + 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); + 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 z_dev_min(0); + if (node.getParam("z_dev_min", z_dev_min)) + { + ROS_INFO("z_dev_min : %f" , z_dev_min); + } else { + node.setParam("z_dev_min", 0.1); + node.getParam("z_dev_min", z_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("/cmd_vel", 1); + 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("input", 1, &Run::callback, &run); + + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&Run::reconfigure, &run, _1, _2); + server.setCallback(f); + + run.run(); + return 0; +}