36 #define BOOST_PARAMETER_MAX_ARITY 7 38 #include <pcl/common/angles.h> 40 #include <std_msgs/Int32.h> 46 DiagnosticNodelet::onInit();
50 pub_pose_ = advertise<geometry_msgs::PoseStamped>(*
pnh_,
"output/pose", 1);
51 pub_mask_ = advertise<sensor_msgs::Image>(*
pnh_,
"output/mask", 1);
54 *
pnh_,
"output/mask_indices", 1);
56 *
pnh_,
"output/left_image", 1);
58 *
pnh_,
"output/left_camera_info", 1);
60 *
pnh_,
"output/right_camera_info", 1);
62 *
pnh_,
"output/disparity", 1);
64 *
pnh_,
"output/count", 1);
69 const geometry_msgs::Pose& new_pose)
71 Eigen::Affine3d new_affine;
73 for (
size_t i = 0; i <
poses_.size(); i++) {
75 Eigen::Affine3d affine;
78 Eigen::Affine3d
diff = affine.inverse() * new_affine;
79 double positional_difference = diff.translation().norm();
81 Eigen::AngleAxisd rotational_difference(diff.rotation());
99 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
130 const geometry_msgs::PoseStamped::ConstPtr& pose,
131 const sensor_msgs::Image::ConstPtr& mask,
132 const PCLIndicesMsg::ConstPtr& mask_indices,
133 const sensor_msgs::Image::ConstPtr& left_image,
134 const sensor_msgs::CameraInfo::ConstPtr& left_cam_info,
135 const sensor_msgs::CameraInfo::ConstPtr& right_cam_info,
136 const stereo_msgs::DisparityImage::ConstPtr& disparity)
143 poses_.push_back(pose->pose);
152 std_msgs::Int32
count;
ros::Publisher pub_count_
void publish(const boost::shared_ptr< M > &message) const
std::vector< geometry_msgs::Pose > poses_
ros::Publisher pub_right_cam_info_
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_right_cam_info_
virtual void unsubscribe()
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
double rotational_bin_size_
double positional_bin_size_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
message_filters::Subscriber< PCLIndicesMsg > sub_mask_indices_
ros::Publisher pub_mask_indices_
ros::Publisher pub_disparity_
virtual bool checkNearPose(const geometry_msgs::Pose &new_pose)
ros::Publisher pub_left_image_
message_filters::Subscriber< sensor_msgs::Image > sub_mask_
message_filters::Subscriber< geometry_msgs::PoseStamped > sub_pose_
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
message_filters::Subscriber< sensor_msgs::Image > sub_left_image_
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_left_cam_info_
ros::Publisher pub_left_cam_info_
double count(const OneDataStat &d)
wrapper function for count method for boost::function
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::CaptureStereoSynchronizer, nodelet::Nodelet)
virtual void republish(const geometry_msgs::PoseStamped::ConstPtr &pose, const sensor_msgs::Image::ConstPtr &mask, const PCLIndicesMsg::ConstPtr &mask_indices, const sensor_msgs::Image::ConstPtr &left_image, const sensor_msgs::CameraInfo::ConstPtr &left_cam_info, const sensor_msgs::CameraInfo::ConstPtr &right_cam_info, const stereo_msgs::DisparityImage::ConstPtr &disparity)
message_filters::Subscriber< stereo_msgs::DisparityImage > sub_disparity_