35 #define BOOST_PARAMETER_MAX_ARITY 7 45 DiagnosticNodelet::onInit();
47 ROS_FATAL(
"~target_frame_id is not specified");
54 pub_ = advertise<jsk_recognition_msgs::BoundingBox>(*
pnh_,
"output", 1);
85 const jsk_recognition_msgs::BoundingBox::ConstPtr& msg)
90 jsk_recognition_msgs::BoundingBox transformed_box;
91 transformed_box.header.stamp = msg->header.stamp;
93 transformed_box.dimensions = msg->dimensions;
101 msg->header.stamp, tf_transform);
103 Eigen::Affine3f
pose;
107 Eigen::Affine3f new_pose = transform * pose;
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
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)
static tf::TransformListener * getInstance()