LGD/hand_control
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

adds Header and timing to estimated plans

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-02 18:02:58 +02:00
parent 08f4e835ab
commit e70baf354f
3 changed files with 15 additions and 3 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
CMakeLists.txt
View file

@ -27,7 +27,9 @@ generate_messages(
geometry_msgs geometry_msgs
) )
catkin_package() catkin_package(
CATKIN_DEPENDS message_runtime
)
include_directories( include_directories(
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}

1
msg/Plan.msg
View file

@ -1,3 +1,4 @@
Header header
geometry_msgs/Point normal geometry_msgs/Point normal
float64 altitude float64 altitude
float64 curvature float64 curvature

13
src/normal_estimator.cpp
View file

@ -3,6 +3,7 @@
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl/features/normal_3d_omp.h> #include <pcl/features/normal_3d_omp.h>
#include <hand_control/Plan.h> #include <hand_control/Plan.h>
#include <time.h>
typedef pcl::PointXYZRGB Point; typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud<Point> PointCloud; typedef pcl::PointCloud<Point> PointCloud;
@ -28,7 +29,7 @@ class Callback {
// publication // publication
ROS_INFO("Plan published"); ROS_INFO("Plan published");
publisher.publish(to_Plan(x, y, z, h, c)); publisher.publish(to_Plan(x, y, z, h, c, msg->header.stamp));
} }
Callback(ros::Publisher& pub) : publisher(pub), estimator() {} Callback(ros::Publisher& pub) : publisher(pub), estimator() {}
@ -36,12 +37,13 @@ class Callback {
private: private:
ros::Publisher publisher; ros::Publisher publisher;
pcl::NormalEstimationOMP<Point, pcl::Normal> estimator; pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
uint32_t number;
inline inline
const hand_control::Plan::ConstPtr const hand_control::Plan::ConstPtr
to_Plan(const float& x, const float& y, to_Plan(const float& x, const float& y,
const float& z, const float& h, const float& z, const float& h,
const float& c) const float& c, uint64_t msec64)
{ {
hand_control::Plan::Ptr ros_msg(new hand_control::Plan()); hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
ros_msg->normal.x = x; ros_msg->normal.x = x;
@ -49,6 +51,13 @@ class Callback {
ros_msg->normal.z = z; ros_msg->normal.z = z;
ros_msg->altitude = h; ros_msg->altitude = h;
ros_msg->curvature = c; ros_msg->curvature = c;
// 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 = number++;
ros_msg->header.frame_id = "0";
return ros_msg; return ros_msg;
} }