38 #include <aruco/aruco.h> 39 #include <aruco/cvdrawingutils.h> 41 #include <opencv2/core/core.hpp> 46 #include <aruco_ros/aruco_ros_utils.h> 50 #include <dynamic_reconfigure/server.h> 51 #include <aruco_ros/ArucoThresholdConfig.h> 54 using namespace aruco;
78 double ticksBefore = cv::getTickCount();
91 ROS_WARN(
"normalizeImageIllumination is unimplemented!");
102 for(
unsigned int i=0; i<
markers.size(); ++i)
110 geometry_msgs::Pose poseMsg;
119 geometry_msgs::Pose poseMsg;
133 float x[2],
y[2], u[2], v[2];
134 for (
unsigned int i = 0; i < 2; ++i)
137 <<
markers[i].Tvec.at<
float>(0,0) <<
", " 138 <<
markers[i].Tvec.at<
float>(1,0) <<
", " 139 <<
markers[i].Tvec.at<
float>(2,0));
141 x[i] =
markers[i].Tvec.at<
float>(0,0)/
markers[i].Tvec.at<
float>(2,0);
142 y[i] =
markers[i].Tvec.at<
float>(1,0)/
markers[i].Tvec.at<
float>(2,0);
151 << (x[0]+x[1])/2 <<
", " << (y[0]+y[1])/2 <<
")");
161 for (
unsigned int i = 0; i < 3; ++i )
162 midPoint3D[i] = (
markers[0].Tvec.at<
float>(i,0) +
163 markers[1].Tvec.at<
float>(i,0) ) / 2;
165 float midPointNormalized[2];
166 midPointNormalized[0] = midPoint3D[0]/midPoint3D[2];
167 midPointNormalized[1] = midPoint3D[1]/midPoint3D[2];
168 u[0] = midPointNormalized[0]*camParam.
CameraMatrix.at<
float>(0,0) +
170 v[0] = midPointNormalized[1]*camParam.
CameraMatrix.at<
float>(1,1) +
173 ROS_DEBUG_STREAM(
"3D Mid point between the two markers in undistorted pixel coordinates = (" 174 << u[0] <<
", " << v[0] <<
")");
178 cv::Point(u[0], v[0]),
179 3, cv::Scalar(0,0,255), CV_FILLED);
186 for(
unsigned int i=0; i<
markers.size(); ++i)
196 out_msg.
header.stamp = curr_stamp;
206 debug_msg.
header.stamp = curr_stamp;
213 1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
217 ROS_ERROR(
"cv_bridge exception: %s", e.what());
244 dynamic_reconfigure::Server<aruco_ros::ArucoThresholdConfig> server;
245 dynamic_reconfigure::Server<aruco_ros::ArucoThresholdConfig>::CallbackType f_;
247 server.setCallback(f_);
260 pose_pub1 = nh.
advertise<geometry_msgs::Pose>(
"pose", 100);
261 pose_pub2 = nh.
advertise<geometry_msgs::Pose>(
"pose2", 100);
268 if(dctComponentsToRemove == 0)
269 normalizeImageIllumination =
false;
274 if(parent_name ==
"" || child_name1 ==
"" || child_name2 ==
"")
276 ROS_ERROR(
"parent_name and/or child_name was not set!");
280 ROS_INFO(
"Aruco node started with marker size of %f meters and marker ids to track: %d, %d",
281 marker_size, marker_id1, marker_id2);
282 ROS_INFO(
"Aruco node will publish pose to TF with (%s, %s) and (%s, %s) as (parent,child).",
283 parent_name.c_str(), child_name1.c_str(), parent_name.c_str(), child_name2.c_str());
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 setThresholdParams(double param1, double param2)
aruco::CameraParameters camParam
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 reconf_callback(aruco_ros::ArucoThresholdConfig &config, uint32_t level)
ros::Subscriber cam_info_sub
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)
TFSIMD_FORCE_INLINE const tfScalar & y() const
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)
image_transport::Publisher debug_pub
tf::Transform arucoMarker2Tf(const aruco::Marker &marker, bool rotate_marker_axis=true)
void publish(const sensor_msgs::Image &message) const
void cam_info_callback(const sensor_msgs::CameraInfo &msg)
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())
TFSIMD_FORCE_INLINE const tfScalar & x() 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 ...
#define ROS_DEBUG_STREAM(args)
const cv::Mat & getThresholdedImage()
Parameters of the camera.
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
#define ROS_INFO_STREAM(args)
int main(int argc, char **argv)
int dctComponentsToRemove
void image_callback(const sensor_msgs::ImageConstPtr &msg)
sensor_msgs::ImagePtr toImageMsg() const
bool normalizeImageIllumination