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;
-}