42 #include <opencv2/core/core.hpp> 44 #include <Eigen/Dense> 45 #include <Eigen/Geometry> 46 #include <Eigen/StdVector> 49 #include "turtlebot_actions/FindFiducialAction.h" 78 cv::Size grid_size(goal->pattern_width,goal->pattern_height);
83 std::string image_topic = goal->camera_name +
"/image_rect_mono";
94 ROS_INFO(
"%s: Aborted, there are publishers on goal topic, but detection took too long.",
action_name_.c_str());
109 void detectCB(
const sensor_msgs::ImageConstPtr& image_msg,
const sensor_msgs::CameraInfoConstPtr& info_msg)
124 ROS_ERROR(
"[calibrate] Failed to convert image");
133 Eigen::Vector3f translation;
134 Eigen::Quaternionf orientation;
148 fiducial_pose = fiducial_transform * fiducial_pose;
178 turtlebot_actions::FindFiducialResult
result_;
184 int main(
int argc,
char** argv)
186 ros::init(argc, argv,
"find_fiducial_pose");
const cv::Matx33d & intrinsicMatrix() const
boost::shared_ptr< const Goal > acceptNewGoal()
void detectCB(const sensor_msgs::ImageConstPtr &image_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
cv_bridge::CvImagePtr img_bridge_
CameraSubscriber subscribeCamera(const std::string &base_topic, uint32_t queue_size, const CameraSubscriber::Callback &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
turtlebot_actions::FindFiducialResult result_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
PatternDetector detector_
ROSCPP_DECL const std::string & getName()
~FindFiducialAction(void)
tf::TransformListener tf_listener_
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
ROSCPP_DECL void spin(Spinner &spinner)
std::string tfFrame() const
image_transport::CameraSubscriber sub_
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
void setPattern(cv::Size grid_size_, float square_size_, Pattern pattern_type_, cv::Point3f offset_=cv::Point3f())
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void registerPreemptCallback(boost::function< void()> cb)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
image_geometry::PinholeCameraModel cam_model_
void setCameraMatrices(cv::Matx33d K_, cv::Matx33d D_)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
image_transport::ImageTransport it_
tf::TransformBroadcaster tf_broadcaster_
GLuint const GLchar * name
actionlib::SimpleActionServer< turtlebot_actions::FindFiducialAction > as_
uint32_t getNumPublishers() const
FindFiducialAction(std::string name)
const cv::Mat_< double > & distortionCoeffs() const
int main(int argc, char **argv)
void timeoutCB(const ros::TimerEvent &e)
void registerGoalCallback(boost::function< void()> cb)
int detectPattern(cv::Mat &inm, Eigen::Vector3f &translation, Eigen::Quaternionf &orientation)
turtlebot_actions::FindFiducialFeedback feedback_