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::BoundingBoxArray>(*
pnh_,
"output", 1);
86 const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& msg)
91 jsk_recognition_msgs::BoundingBoxArray transformed_box;
92 transformed_box.header.stamp = msg->header.stamp;
101 msg->header.stamp, tf_transform);
106 for (
size_t i = 0; i < msg->boxes.size(); i++) {
107 jsk_recognition_msgs::BoundingBox box = jsk_recognition_msgs::BoundingBox(msg->boxes[i]);
108 Eigen::Affine3f
pose;
110 Eigen::Affine3f new_pose = transform * pose;
113 transformed_box.boxes.push_back(box);
#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()