37 #include <aruco/aruco.h> 38 #include <aruco/cvdrawingutils.h> 40 #include <opencv2/core/core.hpp> 45 #include <aruco_ros/aruco_ros_utils.h> 48 #include <visualization_msgs/Marker.h> 50 using namespace aruco;
70 std::string marker_frame;
71 std::string camera_frame;
72 std::string reference_frame;
85 : cam_info_received(false),
90 std::string refinementMethod;
91 nh.
param<std::string>(
"corner_refinement", refinementMethod,
"LINES");
92 if ( refinementMethod ==
"SUBPIX" )
94 else if ( refinementMethod ==
"HARRIS" )
96 else if ( refinementMethod ==
"NONE" )
106 ROS_INFO_STREAM(
"Threshold method: " <<
" th1: " << th1 <<
" th2: " << th2);
119 pose_pub = nh.
advertise<geometry_msgs::PoseStamped>(
"pose", 100);
120 transform_pub = nh.
advertise<geometry_msgs::TransformStamped>(
"transform", 100);
121 position_pub = nh.
advertise<geometry_msgs::Vector3Stamped>(
"position", 100);
122 marker_pub = nh.
advertise<visualization_msgs::Marker>(
"marker", 10);
123 pixel_pub = nh.
advertise<geometry_msgs::PointStamped>(
"pixel", 10);
126 nh.
param<
int>(
"marker_id", marker_id, 300);
127 nh.
param<std::string>(
"reference_frame", reference_frame,
"");
128 nh.
param<std::string>(
"camera_frame", camera_frame,
"");
129 nh.
param<std::string>(
"marker_frame", marker_frame,
"");
132 ROS_ASSERT(camera_frame !=
"" && marker_frame !=
"");
134 if ( reference_frame.empty() )
135 reference_frame = camera_frame;
137 ROS_INFO(
"Aruco node started with marker size of %f m and marker id to track: %d",
138 marker_size, marker_id);
139 ROS_INFO(
"Aruco node will publish pose to TF with %s as parent and %s as child.",
140 reference_frame.c_str(), marker_frame.c_str());
144 const std::string& childFrame,
170 ROS_ERROR_STREAM(
"Error in lookupTransform of " << childFrame <<
" in " << refFrame);
182 if(cam_info_received)
189 inImage = cv_ptr->image;
194 mDetector.
detect(inImage, markers, camParam, marker_size,
false);
196 for(
size_t i=0; i<markers.size(); ++i)
199 if(markers[i].
id == marker_id)
205 if ( reference_frame != camera_frame )
207 getTransform(reference_frame,
214 * static_cast<tf::Transform>(rightToLeft)
218 reference_frame, marker_frame);
220 geometry_msgs::PoseStamped poseMsg;
222 poseMsg.header.frame_id = reference_frame;
223 poseMsg.header.stamp = curr_stamp;
226 geometry_msgs::TransformStamped transformMsg;
228 transform_pub.
publish(transformMsg);
230 geometry_msgs::Vector3Stamped positionMsg;
231 positionMsg.header = transformMsg.header;
232 positionMsg.vector = transformMsg.transform.translation;
233 position_pub.
publish(positionMsg);
235 geometry_msgs::PointStamped pixelMsg;
236 pixelMsg.header = transformMsg.header;
237 pixelMsg.point.x = markers[i].getCenter().x;
238 pixelMsg.point.y = markers[i].getCenter().y;
239 pixelMsg.point.z = 0;
243 visualization_msgs::Marker visMarker;
244 visMarker.header = transformMsg.header;
245 visMarker.pose = poseMsg.pose;
247 visMarker.type = visualization_msgs::Marker::CUBE;
248 visMarker.action = visualization_msgs::Marker::ADD;
249 visMarker.pose = poseMsg.pose;
251 visMarker.scale.y = 0.001;
253 visMarker.color.r = 1.0;
254 visMarker.color.g = 0;
255 visMarker.color.b = 0;
256 visMarker.color.a = 1.0;
262 markers[i].draw(inImage,cv::Scalar(0,0,255),2);
266 if(camParam.
isValid() && marker_size!=-1)
268 for(
size_t i=0; i<markers.size(); ++i)
278 out_msg.
header.stamp = curr_stamp;
288 debug_msg.
header.stamp = curr_stamp;
296 ROS_ERROR(
"cv_bridge exception: %s", e.what());
316 cam_info_received =
true;
bool getTransform(const std::string &refFrame, const std::string &childFrame, tf::StampedTransform &transform)
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
void detect(const cv::Mat &input, std::vector< Marker > &detectedMarkers, cv::Mat camMatrix=cv::Mat(), cv::Mat distCoeff=cv::Mat(), float markerSizeMeters=-1, bool setYPerpendicular=true)
void getMinMaxSize(float &min, float &max)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void getThresholdParams(double ¶m1, double ¶m2) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
CornerRefinementMethod getCornerRefinementMethod() const
void image_callback(const sensor_msgs::ImageConstPtr &msg)
uint32_t getNumSubscribers() const
ROSCPP_DECL void spin(Spinner &spinner)
pcl::PointCloud< myPointXYZRID > transform(pcl::PointCloud< myPointXYZRID > pc, float x, float y, float z, float rot_x, float rot_y, float rot_z)
ros::Subscriber cam_info_sub
int main(int argc, char **argv)
tf::Transform arucoMarker2Tf(const aruco::Marker &marker, bool rotate_marker_axis=true)
void publish(const sensor_msgs::Image &message) const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
image_transport::Publisher image_pub
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
image_transport::Publisher debug_pub
static void draw3dAxis(cv::Mat &Image, Marker &m, const CameraParameters &CP)
ThresholdMethods getThresholdMethod() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Main class for marker detection.
aruco::CameraParameters rosCameraInfo2ArucoCamParams(const sensor_msgs::CameraInfo &cam_info, bool useRectifiedParameters)
rosCameraInfo2ArucoCamParams gets the camera intrinsics from a CameraInfo message and copies them to ...
const cv::Mat & getThresholdedImage()
Parameters of the camera.
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
#define ROS_INFO_STREAM(args)
int getDesiredSpeed() const
void cam_info_callback(const sensor_msgs::CameraInfo &msg)
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
#define ROS_ERROR_STREAM(args)
void setCornerRefinementMethod(CornerRefinementMethod method)
sensor_msgs::ImagePtr toImageMsg() const
aruco::CameraParameters camParam