This commit is contained in:
_Luc_ 2015-05-06 18:46:40 +02:00
commit f099f5854a
3 changed files with 123 additions and 4 deletions

View file

@ -50,6 +50,9 @@ add_library(display src/display.cpp)
add_executable(keyboard_cmd src/keyboard_cmd.cpp) add_executable(keyboard_cmd src/keyboard_cmd.cpp)
target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRARIES}) target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRARIES})
add_executable(filtreRGB src/filtreRGB.cpp)
target_link_libraries(filtreRGB ${catkin_LIBRARIES})
add_executable(normal_estimator src/normal_estimator.cpp) add_executable(normal_estimator src/normal_estimator.cpp)
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)

116
src/filtreRGB.cpp Normal file
View file

@ -0,0 +1,116 @@
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud<Point> PointCloud;
class Callback {
public:
void
operator()(const PointCloud::ConstPtr& msg)
{
PointCloud::Ptr pcl(new PointCloud());
copy_info(msg, pcl);
BOOST_FOREACH (const Point& pt, msg->points)
{
if (abs(pt.r-red) <= delta and abs(pt.g-green) <= delta and abs(pt.b-blue) <= delta)
pcl->push_back(pt);
}
pcl->height = 1;
pcl->width = pcl->points.size();
publisher.publish(pcl);
}
Callback(ros::Publisher& pub, uint8_t r, uint8_t g, uint8_t b, uint8_t d)
: publisher(pub), delta(d), red(r), green(g), blue(b) {}
private:
ros::Publisher publisher;
uint8_t delta;
uint8_t red;
uint8_t green;
uint8_t blue;
inline
void
copy_info(const PointCloud::ConstPtr& a,
PointCloud::Ptr b)
{
b->header = a->header;
b->sensor_origin_ = a->sensor_origin_;
b->sensor_orientation_ = a->sensor_orientation_;
b->is_dense = a->is_dense;
}
};
int
main(int argc, char** argv)
{
ros::init(argc, argv, "filtreRGB");
ros::NodeHandle node("filtreRGB");
// récupération des paramètres
uint8_t red(0);
uint8_t blue(0);
uint8_t green(0);
uint8_t delta(0);
int redInt;
int greenInt;
int blueInt;
int deltaInt;
if (node.getParam("red", redInt))
{
ROS_INFO("red : %d" , redInt);
} else {
node.setParam("red", 0);
node.getParam("red", redInt);
ROS_INFO("red : %d (default value)", redInt);
}
if (node.getParam("blue", blueInt))
{
ROS_INFO("blue : %d" , blueInt);
} else {
node.setParam("blue", 0);
node.getParam("blue", blueInt);
ROS_INFO("blue : %d (default value)", blueInt);
}
if (node.getParam("green", greenInt))
{
ROS_INFO("green : %d" , greenInt);
} else {
node.setParam("green", 0);
node.getParam("green", greenInt);
ROS_INFO("green : %d (default value)", greenInt);
}
if (node.getParam("delta", deltaInt))
{
ROS_INFO("delta : %d" , deltaInt);
} else {
node.setParam("delta", 0);
node.getParam("delta", deltaInt);
ROS_INFO("delta : %d (default value)", deltaInt);
}
// initialisatio
ros::Publisher publisher = node.advertise<PointCloud>("output", 1);
Callback callback(publisher, (uint8_t) redInt, (uint8_t) greenInt, (uint8_t) blueInt, (uint8_t) deltaInt);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
// démarrage
ROS_INFO("node started");
ros::spin();
ROS_INFO("exit");
return 0;
}

View file

@ -57,10 +57,10 @@ class Run
cmdvel_callback(terminal), cmdvel_callback(terminal),
term(terminal), term(terminal),
loop_rate(30), loop_rate(30),
x_speed(0.2), x_speed(0.05),
y_speed(0.3), y_speed(0.05),
z_speed(0.5), z_speed(0.05),
turn(0.5) { turn(0.1) {
cmd = n.advertise<geometry_msgs::Twist>("/cmd_vel",1); cmd = n.advertise<geometry_msgs::Twist>("/cmd_vel",1);
pub_takeoff = n.advertise<std_msgs::Empty>("/ardrone/takeoff", 1); pub_takeoff = n.advertise<std_msgs::Empty>("/ardrone/takeoff", 1);
pub_land = n.advertise<std_msgs::Empty>("/ardrone/land", 1); pub_land = n.advertise<std_msgs::Empty>("/ardrone/land", 1);