From 9c1266177981e34fe4d9bc50d2a3b08c8eddb241 Mon Sep 17 00:00:00 2001 From: badrobot Date: Thu, 28 Jun 2018 11:59:30 +0530 Subject: [PATCH 1/3] Added topic to track pixel position of the center of marker --- src/ros/whycon_ros.cpp | 121 ++++++++++++++++++++++++----------------- src/ros/whycon_ros.h | 10 +++- 2 files changed, 77 insertions(+), 54 deletions(-) diff --git a/src/ros/whycon_ros.cpp b/src/ros/whycon_ros.cpp index 55e32bf..dbea7e7 100644 --- a/src/ros/whycon_ros.cpp +++ b/src/ros/whycon_ros.cpp @@ -4,35 +4,37 @@ #include #include #include +#include #include #include #include "whycon_ros.h" +using std::cout; whycon::WhyConROS::WhyConROS(ros::NodeHandle& n) : is_tracking(false), should_reset(true), it(n) { - transformation_loaded = false; - similarity.setIdentity(); + transformation_loaded = false; + similarity.setIdentity(); if (!n.getParam("targets", targets)) throw std::runtime_error("Private parameter \"targets\" is missing"); n.param("name", frame_id, std::string("whycon")); - n.param("world_frame", world_frame_id, std::string("world")); + n.param("world_frame", world_frame_id, std::string("world")); n.param("max_attempts", max_attempts, 1); n.param("max_refine", max_refine, 1); - n.getParam("outer_diameter", parameters.outer_diameter); - n.getParam("inner_diameter", parameters.inner_diameter); - n.getParam("center_distance_tolerance_abs", parameters.center_distance_tolerance_abs); - n.getParam("center_distance_tolerance_ratio", parameters.center_distance_tolerance_ratio); - n.getParam("roundness_tolerance", parameters.roundness_tolerance); - n.getParam("circularity_tolerance", parameters.circularity_tolerance); - n.getParam("max_size", parameters.max_size); - n.getParam("min_size", parameters.min_size); - n.getParam("ratio_tolerance", parameters.ratio_tolerance); - n.getParam("max_eccentricity", parameters.max_eccentricity); + n.getParam("outer_diameter", parameters.outer_diameter); + n.getParam("inner_diameter", parameters.inner_diameter); + n.getParam("center_distance_tolerance_abs", parameters.center_distance_tolerance_abs); + n.getParam("center_distance_tolerance_ratio", parameters.center_distance_tolerance_ratio); + n.getParam("roundness_tolerance", parameters.roundness_tolerance); + n.getParam("circularity_tolerance", parameters.circularity_tolerance); + n.getParam("max_size", parameters.max_size); + n.getParam("min_size", parameters.min_size); + n.getParam("ratio_tolerance", parameters.ratio_tolerance); + n.getParam("max_eccentricity", parameters.max_eccentricity); - load_transforms(); - transform_broadcaster = boost::make_shared(); + load_transforms(); + transform_broadcaster = boost::make_shared(); /* initialize ros */ int input_queue_size = 1; @@ -42,7 +44,9 @@ whycon::WhyConROS::WhyConROS(ros::NodeHandle& n) : is_tracking(false), should_re image_pub = n.advertise("image_out", 1); poses_pub = n.advertise("poses", 1); context_pub = n.advertise("context", 1); - projection_pub = n.advertise("projection", 1); + projection_pub = n.advertise("projection", 1); + + circlecenter_pub = n.advertise("circle_center", 1); reset_service = n.advertiseService("reset", &WhyConROS::reset, this); } @@ -76,9 +80,12 @@ void whycon::WhyConROS::on_image(const sensor_msgs::ImageConstPtr& image_msg, co } } -bool whycon::WhyConROS::reset(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) +bool whycon::WhyConROS::reset(whycon::SetNumberOfTargets::Request& request, whycon::SetNumberOfTargets::Response& response) { should_reset = true; + targets = (int)request.number; + response.targets = targets; + system = 0; return true; } @@ -95,22 +102,34 @@ void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv output_image = cv_ptr->image.clone(); geometry_msgs::PoseArray pose_array; - + geometry_msgs::Pose circle_pose; // go through detected targets + for (int i = 0; i < system->targets; i++) { const whycon::CircleDetector::Circle& circle = system->get_circle(i); + + // cout<get_pose(circle); cv::Vec3f coord = pose.pos; + // draw each target if (publish_images) { std::ostringstream ostr; ostr << std::fixed << std::setprecision(2); - ostr << coord << " " << i; + ostr << coord << " " << i; circle.draw(output_image, ostr.str(), cv::Vec3b(0,255,255)); - /*whycon::CircleDetector::Circle new_circle = circle.improveEllipse(cv_ptr->image); - new_circle.draw(output_image, ostr.str(), cv::Vec3b(0,255,0));*/ - cv::circle(output_image, camera_model.project3dToPixel(cv::Point3d(coord)), 1, cv::Scalar(255,0,255), 1, CV_AA); + // whycon::CircleDetector::Circle new_circle = circle.improveEllipse(cv_ptr->image); + // new_circle.draw(output_image, ostr.str(), cv::Vec3b(0,255,0)); + cv::circle(output_image, camera_model.project3dToPixel(cv::Point3d(coord)), 1, cv::Scalar(255,0,255), 1, CV_AA); } if (publish_poses) { @@ -135,49 +154,49 @@ void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv poses_pub.publish(pose_array); } - if (transformation_loaded) - { - transform_broadcaster->sendTransform(tf::StampedTransform(similarity, header.stamp, world_frame_id, frame_id)); + // if (transformation_loaded) + // { + // transform_broadcaster->sendTransform(tf::StampedTransform(similarity, header.stamp, world_frame_id, frame_id)); - whycon::Projection projection_msg; - projection_msg.header = header; - for (size_t i = 0; i < projection.size(); i++) projection_msg.projection[i] = projection[i]; - projection_pub.publish(projection_msg); - } + // whycon::Projection projection_msg; + // projection_msg.header = header; + // for (size_t i = 0; i < projection.size(); i++) projection_msg.projection[i] = projection[i]; + // projection_pub.publish(projection_msg); + // } } void whycon::WhyConROS::load_transforms(void) { - std::string filename = frame_id + "_transforms.yml"; - ROS_INFO_STREAM("Loading file " << filename); + std::string filename = frame_id + "_transforms.yml"; + ROS_INFO_STREAM("Loading file " << filename); - std::ifstream file(filename); - if (!file) { - ROS_WARN_STREAM("Could not load \"" << filename << "\""); - return; - } + std::ifstream file(filename); + if (!file) { + ROS_WARN_STREAM("Could not load \"" << filename << "\""); + return; + } - YAML::Node node = YAML::Load(file); + YAML::Node node = YAML::Load(file); - projection.resize(9); - for (int i = 0; i < 9; i++) - projection[i] = node["projection"][i].as(); + projection.resize(9); + for (int i = 0; i < 9; i++) + projection[i] = node["projection"][i].as(); - std::vector similarity_origin(3); - for (int i = 0; i < 3; i++) similarity_origin[i] = node["similarity"]["origin"][i].as(); + std::vector similarity_origin(3); + for (int i = 0; i < 3; i++) similarity_origin[i] = node["similarity"]["origin"][i].as(); - std::vector similarity_rotation(4); - for (int i = 0; i < 4; i++) similarity_rotation[i] = node["similarity"]["rotation"][i].as(); + std::vector similarity_rotation(4); + for (int i = 0; i < 4; i++) similarity_rotation[i] = node["similarity"]["rotation"][i].as(); - tf::Vector3 origin(similarity_origin[0], similarity_origin[1], similarity_origin[2]); - tf::Quaternion Q(similarity_rotation[0], similarity_rotation[1], similarity_rotation[2], similarity_rotation[3]); + tf::Vector3 origin(similarity_origin[0], similarity_origin[1], similarity_origin[2]); + tf::Quaternion Q(similarity_rotation[0], similarity_rotation[1], similarity_rotation[2], similarity_rotation[3]); - similarity.setOrigin(origin); - similarity.setRotation(Q); + similarity.setOrigin(origin); + similarity.setRotation(Q); - transformation_loaded = true; + transformation_loaded = true; - ROS_INFO_STREAM("Loaded transformation from \"" << filename << "\""); + ROS_INFO_STREAM("Loaded transformation from \"" << filename << "\""); } diff --git a/src/ros/whycon_ros.h b/src/ros/whycon_ros.h index 6c1f2be..c1e02ba 100644 --- a/src/ros/whycon_ros.h +++ b/src/ros/whycon_ros.h @@ -5,9 +5,9 @@ #include #include #include -#include #include #include +#include "whycon/SetNumberOfTargets.h" namespace whycon { class WhyConROS { @@ -15,7 +15,7 @@ namespace whycon { WhyConROS(ros::NodeHandle& n); void on_image(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); - bool reset(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); + bool reset(whycon::SetNumberOfTargets::Request& request, whycon::SetNumberOfTargets::Response& response); private: void load_transforms(void); @@ -28,6 +28,10 @@ namespace whycon { std::string world_frame_id, frame_id; int targets; double xscale, yscale; + float ground_robot_threshold; + int marker_no; + float marker_tresh; + float out_condition; std::vector projection; tf::Transform similarity; @@ -36,7 +40,7 @@ namespace whycon { image_transport::CameraSubscriber cam_sub; ros::ServiceServer reset_service; - ros::Publisher image_pub, poses_pub, context_pub, projection_pub; + ros::Publisher image_pub, poses_pub, context_pub, projection_pub, circlecenter_pub; boost::shared_ptr transform_broadcaster; image_geometry::PinholeCameraModel camera_model; From a684a668fa0d041e3c6a7ebe066896e9eea43ae5 Mon Sep 17 00:00:00 2001 From: badrobot Date: Fri, 29 Jun 2018 12:45:33 +0530 Subject: [PATCH 2/3] Changed Pose to PoseArray --- src/ros/whycon_ros.cpp | 42 +++++++++++++++++++----------------------- src/ros/whycon_ros.h | 10 +++------- 2 files changed, 22 insertions(+), 30 deletions(-) diff --git a/src/ros/whycon_ros.cpp b/src/ros/whycon_ros.cpp index dbea7e7..c2d7bc6 100644 --- a/src/ros/whycon_ros.cpp +++ b/src/ros/whycon_ros.cpp @@ -4,11 +4,9 @@ #include #include #include -#include #include #include #include "whycon_ros.h" -using std::cout; whycon::WhyConROS::WhyConROS(ros::NodeHandle& n) : is_tracking(false), should_reset(true), it(n) { @@ -46,7 +44,7 @@ whycon::WhyConROS::WhyConROS(ros::NodeHandle& n) : is_tracking(false), should_re context_pub = n.advertise("context", 1); projection_pub = n.advertise("projection", 1); - circlecenter_pub = n.advertise("circle_center", 1); + circlecenter_pub = n.advertise("circle_center", 1); reset_service = n.advertiseService("reset", &WhyConROS::reset, this); } @@ -80,12 +78,9 @@ void whycon::WhyConROS::on_image(const sensor_msgs::ImageConstPtr& image_msg, co } } -bool whycon::WhyConROS::reset(whycon::SetNumberOfTargets::Request& request, whycon::SetNumberOfTargets::Response& response) +bool whycon::WhyConROS::reset(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { should_reset = true; - targets = (int)request.number; - response.targets = targets; - system = 0; return true; } @@ -102,19 +97,20 @@ void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv output_image = cv_ptr->image.clone(); geometry_msgs::PoseArray pose_array; - geometry_msgs::Pose circle_pose; + geometry_msgs::PoseArray circle_pose; // go through detected targets for (int i = 0; i < system->targets; i++) { const whycon::CircleDetector::Circle& circle = system->get_circle(i); - // cout<get_pose(circle); @@ -154,15 +150,15 @@ void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv poses_pub.publish(pose_array); } - // if (transformation_loaded) - // { - // transform_broadcaster->sendTransform(tf::StampedTransform(similarity, header.stamp, world_frame_id, frame_id)); + if (transformation_loaded) + { + transform_broadcaster->sendTransform(tf::StampedTransform(similarity, header.stamp, world_frame_id, frame_id)); - // whycon::Projection projection_msg; - // projection_msg.header = header; - // for (size_t i = 0; i < projection.size(); i++) projection_msg.projection[i] = projection[i]; - // projection_pub.publish(projection_msg); - // } + whycon::Projection projection_msg; + projection_msg.header = header; + for (size_t i = 0; i < projection.size(); i++) projection_msg.projection[i] = projection[i]; + projection_pub.publish(projection_msg); + } } void whycon::WhyConROS::load_transforms(void) diff --git a/src/ros/whycon_ros.h b/src/ros/whycon_ros.h index c1e02ba..b353a67 100644 --- a/src/ros/whycon_ros.h +++ b/src/ros/whycon_ros.h @@ -5,9 +5,9 @@ #include #include #include +#include #include #include -#include "whycon/SetNumberOfTargets.h" namespace whycon { class WhyConROS { @@ -15,7 +15,7 @@ namespace whycon { WhyConROS(ros::NodeHandle& n); void on_image(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg); - bool reset(whycon::SetNumberOfTargets::Request& request, whycon::SetNumberOfTargets::Response& response); + bool reset(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); private: void load_transforms(void); @@ -28,11 +28,7 @@ namespace whycon { std::string world_frame_id, frame_id; int targets; double xscale, yscale; - float ground_robot_threshold; - int marker_no; - float marker_tresh; - float out_condition; - + std::vector projection; tf::Transform similarity; From fa8e50fdac83384f323c5de2e13bbc595f48dd40 Mon Sep 17 00:00:00 2001 From: Vikrant Fernandes Date: Wed, 24 Oct 2018 14:35:42 +0530 Subject: [PATCH 3/3] Update whycon.launch --- launch/whycon.launch | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/launch/whycon.launch b/launch/whycon.launch index 0f69e31..e0a996a 100644 --- a/launch/whycon.launch +++ b/launch/whycon.launch @@ -1,16 +1,25 @@ - - - - + + + + + + + + + + - + + + + + -