From 6f4e6eda41ca2031932d2b57a3239c49e9f7c85c Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Wed, 27 May 2015 22:27:57 +0200 Subject: [PATCH] better english node and file naming, & rm useless files --- CMakeLists.txt | 24 +- cfg/{Commande.cfg => Commander.cfg} | 2 +- cfg/{Filtre.cfg => Filter.cfg} | 2 +- launch/all-abs.launch | 19 -- launch/all-new-1d.launch | 19 -- launch/all-new.launch | 20 -- launch/all-param.launch | 25 -- launch/all.launch | 19 -- launch/ardrone_aggressive.launch | 75 ------ launch/essai-commande.launch | 26 -- launch/essai-estimator.launch | 10 - launch/essai-filtre.launch | 11 - launch/kinect_commander.launch | 17 ++ src/commande-new-1d.cpp | 230 ------------------ src/{commande-abs.cpp => commander.cpp} | 12 +- ...normal_estimator-pca.cpp => estimator.cpp} | 0 src/{filtre.cpp => filter.cpp} | 12 +- src/normal_estimator.cpp | 94 ------- 18 files changed, 43 insertions(+), 574 deletions(-) rename cfg/{Commande.cfg => Commander.cfg} (95%) rename cfg/{Filtre.cfg => Filter.cfg} (92%) delete mode 100644 launch/all-abs.launch delete mode 100644 launch/all-new-1d.launch delete mode 100644 launch/all-new.launch delete mode 100644 launch/all-param.launch delete mode 100644 launch/all.launch delete mode 100644 launch/ardrone_aggressive.launch delete mode 100644 launch/essai-commande.launch delete mode 100644 launch/essai-estimator.launch delete mode 100644 launch/essai-filtre.launch create mode 100644 launch/kinect_commander.launch delete mode 100644 src/commande-new-1d.cpp rename src/{commande-abs.cpp => commander.cpp} (89%) rename src/{normal_estimator-pca.cpp => estimator.cpp} (100%) rename src/{filtre.cpp => filter.cpp} (90%) delete mode 100644 src/normal_estimator.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e9a230f..9cf5d97 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,8 +35,8 @@ pkg_check_modules ( ncursesw REQUIRED ncursesw) ${ncursesw_INCLUDE_DIRS} ) - add_executable(filtre src/filtre.cpp) -target_link_libraries(filtre ${catkin_LIBRARIES}) + add_executable(filter src/filter.cpp) +target_link_libraries(filter ${catkin_LIBRARIES}) add_executable(random_pcl_publisher src/random_pcl_publisher.cpp) target_link_libraries(random_pcl_publisher ${catkin_LIBRARIES}) @@ -49,22 +49,22 @@ add_library(display src/display.cpp) add_executable(keyboard_cmd src/keyboard_cmd.cpp) target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRARIES}) - add_executable(normal_estimator_pca src/normal_estimator-pca.cpp) -target_link_libraries(normal_estimator_pca ${catkin_LIBRARIES}) -add_dependencies(normal_estimator_pca hand_control_generate_messages_cpp) + add_executable(estimator src/estimator.cpp) +target_link_libraries(estimator ${catkin_LIBRARIES}) +add_dependencies(estimator hand_control_generate_messages_cpp) - add_executable(commande-abs src/commande-abs.cpp) -target_link_libraries(commande-abs ${catkin_LIBRARIES}) -add_dependencies(commande-abs hand_control_generate_messages_cpp) + add_executable(commander src/commander.cpp) +target_link_libraries(commander ${catkin_LIBRARIES}) +add_dependencies(commander hand_control_generate_messages_cpp) #add dynamic reconfigure api generate_dynamic_reconfigure_options( - cfg/Filtre.cfg - cfg/Commande.cfg + cfg/Filter.cfg + cfg/Commander.cfg ) -add_dependencies(filtre ${PROJECT_NAME}_gencfg) -add_dependencies(commande-abs ${PROJECT_NAME}_gencfg) +add_dependencies(filter ${PROJECT_NAME}_gencfg) +add_dependencies(commander ${PROJECT_NAME}_gencfg) catkin_package( CATKIN_DEPENDS message_runtime diff --git a/cfg/Commande.cfg b/cfg/Commander.cfg similarity index 95% rename from cfg/Commande.cfg rename to cfg/Commander.cfg index e48778a..6fbf640 100755 --- a/cfg/Commande.cfg +++ b/cfg/Commander.cfg @@ -13,4 +13,4 @@ gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detec gen.add("angle_vel", double_t, 0, "Angular velocity", 0.5, 0., 10.) gen.add("plan_vel", double_t, 0, "Translation velocity", 2, 0., 10.) gen.add("z_vel", double_t, 0, "Vertical translation velocity", 2, 0., 10.) -exit(gen.generate(PACKAGE, "hand_control", "Commande")) +exit(gen.generate(PACKAGE, "hand_control", "Commander")) diff --git a/cfg/Filtre.cfg b/cfg/Filter.cfg similarity index 92% rename from cfg/Filtre.cfg rename to cfg/Filter.cfg index e064a5e..5ca5303 100755 --- a/cfg/Filtre.cfg +++ b/cfg/Filter.cfg @@ -9,4 +9,4 @@ gen.add("sat_min", double_t, 0, "The minimum color saturation of the hand", 0.1, gen.add("sat_max", double_t, 0, "The maximum color saturation of the hand", 1., 0., 1.) gen.add("val_min", double_t, 0, "The minimum color value of the hand", 0.1, 0., 1.) gen.add("val_max", double_t, 0, "The maximum color value of the hand", 1., 0., 1.) -exit(gen.generate(PACKAGE, "hand_control", "Filtre")) +exit(gen.generate(PACKAGE, "hand_control", "Filter")) diff --git a/launch/all-abs.launch b/launch/all-abs.launch deleted file mode 100644 index b39c50d..0000000 --- a/launch/all-abs.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/launch/all-new-1d.launch b/launch/all-new-1d.launch deleted file mode 100644 index e4e70f6..0000000 --- a/launch/all-new-1d.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/launch/all-new.launch b/launch/all-new.launch deleted file mode 100644 index ca8e5c6..0000000 --- a/launch/all-new.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/launch/all-param.launch b/launch/all-param.launch deleted file mode 100644 index fc2d503..0000000 --- a/launch/all-param.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/all.launch b/launch/all.launch deleted file mode 100644 index e4e70f6..0000000 --- a/launch/all.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/launch/ardrone_aggressive.launch b/launch/ardrone_aggressive.launch deleted file mode 100644 index 185911e..0000000 --- a/launch/ardrone_aggressive.launch +++ /dev/null @@ -1,75 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1] - [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 100000.0] - - diff --git a/launch/essai-commande.launch b/launch/essai-commande.launch deleted file mode 100644 index 2983ba1..0000000 --- a/launch/essai-commande.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/essai-estimator.launch b/launch/essai-estimator.launch deleted file mode 100644 index bc91dd9..0000000 --- a/launch/essai-estimator.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/launch/essai-filtre.launch b/launch/essai-filtre.launch deleted file mode 100644 index 45038a9..0000000 --- a/launch/essai-filtre.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/launch/kinect_commander.launch b/launch/kinect_commander.launch new file mode 100644 index 0000000..c306af4 --- /dev/null +++ b/launch/kinect_commander.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/commande-new-1d.cpp b/src/commande-new-1d.cpp deleted file mode 100644 index 7c770c5..0000000 --- a/src/commande-new-1d.cpp +++ /dev/null @@ -1,230 +0,0 @@ -#include -#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, up_factor; - - // 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) - { - if (dz > 0) - mvt->linear.z = dz * z_vel * up_factor ; - else - mvt->linear.z = dz * 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; - } - - 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& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation, - const float& dz_minimal_difference, const int& min_points_number, const float& up_fact) : - 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), - up_factor(up_fact) { - 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); - } - - 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, x_dev_min, y_dev_min, dz_dev_min, min_number, up_fact); - ros::Subscriber plan_sub = node.subscribe("input", 1, &Run::callback, &run); - run.run(); - return 0; -} diff --git a/src/commande-abs.cpp b/src/commander.cpp similarity index 89% rename from src/commande-abs.cpp rename to src/commander.cpp index 85b2cd1..c09cec7 100644 --- a/src/commande-abs.cpp +++ b/src/commander.cpp @@ -13,7 +13,7 @@ #include #include -#include +#include class Run @@ -111,7 +111,7 @@ class Run publish(); }; - void reconfigure(const hand_control::CommandeConfig& c, const uint32_t& level) { + void reconfigure(const hand_control::CommanderConfig& c, const uint32_t& level) { max_curv = c.max_curvature; x_dev_min = c.x_minimal_deviation; y_dev_min = c.y_minimal_deviation; @@ -134,15 +134,15 @@ class Run int main(int argc, char** argv) { - ros::init(argc, argv, "commande"); - ros::NodeHandle node("commande"); + ros::init(argc, argv, "commander"); + ros::NodeHandle node("commander"); ros::Publisher cmd_pub = node.advertise("/cmd_vel", 1); Run run(cmd_pub); ros::Subscriber plan_sub = node.subscribe("input", 1, &Run::callback, &run); - dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; f = boost::bind(&Run::reconfigure, &run, _1, _2); server.setCallback(f); diff --git a/src/normal_estimator-pca.cpp b/src/estimator.cpp similarity index 100% rename from src/normal_estimator-pca.cpp rename to src/estimator.cpp diff --git a/src/filtre.cpp b/src/filter.cpp similarity index 90% rename from src/filtre.cpp rename to src/filter.cpp index 232d905..6bb18b6 100644 --- a/src/filtre.cpp +++ b/src/filter.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include typedef pcl::PointXYZRGB Point; @@ -43,7 +43,7 @@ class Callback { } void - reconfigure(const hand_control::FiltreConfig& c, const uint32_t& level) { + reconfigure(const hand_control::FilterConfig& c, const uint32_t& level) { z_max = c.z_max; hue = c.hue; delta_hue = c.delta_hue; @@ -95,16 +95,16 @@ class Callback { int main(int argc, char** argv) { - ros::init(argc, argv, "filtre"); - ros::NodeHandle node("filtre"); + ros::init(argc, argv, "filter"); + ros::NodeHandle node("filter"); // initialisation ros::Publisher publisher = node.advertise("output", 1); Callback my_callback(publisher); ros::Subscriber subscriber = node.subscribe("input", 1, &Callback::callback, &my_callback); - dynamic_reconfigure::Server server; - dynamic_reconfigure::Server::CallbackType f; + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; f = boost::bind(&Callback::reconfigure, &my_callback, _1, _2); server.setCallback(f); diff --git a/src/normal_estimator.cpp b/src/normal_estimator.cpp deleted file mode 100644 index 8cc2f63..0000000 --- a/src/normal_estimator.cpp +++ /dev/null @@ -1,94 +0,0 @@ -#include -#include -#include -#include -#include -#include - -typedef pcl::PointXYZRGB Point; -typedef pcl::PointCloud PointCloud; - -class Callback { - public: - void - operator()(const PointCloud::ConstPtr& msg) - { - ROS_INFO("PointCloud received"); - - float x, y, z, h, c; - x = y = z = h = c = 0.; - - // indices : tout le PointCloud - std::vector indices; - for (int i = 0; i < msg->points.size(); ++i) - indices.push_back(i); - - // vérifier signature - estimator.computePointNormal(*msg, indices, x, y, z, c); - h = altitude(msg); - - // publication - ROS_INFO("Plan published"); - publisher.publish(to_Plan(x, y, z, h, c, msg->header.seq, msg->header.stamp, msg->width)); - } - - Callback(ros::Publisher& pub) : - publisher(pub), estimator() {} - - private: - ros::Publisher publisher; - pcl::NormalEstimationOMP estimator; - - inline - const hand_control::Plan::ConstPtr - to_Plan(const float& x, const float& y, - const float& z, const float& h, - const float& c, const uint32_t& seq, - const uint64_t& msec64, const uint64_t& number) - { - hand_control::Plan::Ptr ros_msg(new hand_control::Plan()); - ros_msg->normal.x = x; - ros_msg->normal.y = y; - ros_msg->normal.z = z; - ros_msg->altitude = h; - ros_msg->curvature = c; - ros_msg->number = number; - // uint64_t msec64 is in ms (10-6) - uint64_t sec64 = msec64 / 1000000; - uint64_t nsec64 = (msec64 % 1000000) * 1000; - ros_msg->header.stamp.sec = (uint32_t) sec64; - ros_msg->header.stamp.nsec = (uint32_t) nsec64; - ros_msg->header.seq = seq; - ros_msg->header.frame_id = "0"; - return ros_msg; - } - - inline - float - altitude(const PointCloud::ConstPtr& pcl) - { - int s = pcl->points.size(); - float h(0); - for (int i = 0; i < s; ++i) - h += pcl->points[i].z; - return h/( (float) s ); - } -}; - -int -main(int argc, char** argv) -{ - ros::init(argc, argv, "normal_estimator"); - ros::NodeHandle node("estimator"); - - // initialisation - ros::Publisher publisher = node.advertise("output", 1); - Callback callback(publisher); - ros::Subscriber subscriber = node.subscribe("input", 1, callback); - - // démarrage - ROS_INFO("node started"); - ros::spin(); - ROS_INFO("exit"); - return 0; -}