Commit 9d02516b authored by CANNERE Xavier's avatar CANNERE Xavier
Browse files

6.1.9

parent 6189047f
......@@ -431,12 +431,148 @@ We need to declare variable for the point. Add that before the last``}`` of the
std::uint8_t r;
std::uint8_t seuil;
We can't change variables for the moment, to do that we need to add parameters, first add this in the constructor of the nodes::
this->declare_parameter<std::uint8_t>("blue", 180);
this->declare_parameter<std::uint8_t>("green", 100);
this->declare_parameter<std::uint8_t>("red", 50);
this->declare_parameter<std::uint8_t>("seuil", 30);
And add this before ``CvPoint point`` in the method getImage::
this->get_parameter("blue", b);
this->get_parameter("green", g);
this->get_parameter("red", r);
this->get_parameter("seuil", seuil);
You should have this code::
#include <cstdio>
#include "opencv2/opencv.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "cv_bridge/cv_bridge.h"
#include "DetectionCnam_codels.hpp"
static const std::string OPENCV_WINDOW = "Image window";
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr img_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_;
class Visual : public rclcpp::Node
{
public:
Visual()
: Node("visu")
{
this->declare_parameter<std::uint8_t>("blue", 180);
this->declare_parameter<std::uint8_t>("green", 100);
this->declare_parameter<std::uint8_t>("red", 50);
this->declare_parameter<std::uint8_t>("seuil", 30);
auto sensor_qos = rclcpp::QoS(rclcpp::SensorDataQoS());
img_ = this->create_subscription<sensor_msgs::msg::Image>(
"/camera/rgb/image_raw", sensor_qos,
std::bind(&Visual::getImage, this, std::placeholders::_1));
pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 1);
}
private:
void getImage(const sensor_msgs::msg::Image::SharedPtr _msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(_msg, sensor_msgs::image_encodings::BGR8);
//RCLCPP_INFO("I have received image! ;-)");
}
catch (cv_bridge::Exception& e)
{
//RCLCPP_ERROR("cv_bridge exception: %s", e.what());
return;
}
#if CV_VERSION_MAJOR == 4
IplImage _ipl_img=cvIplImage(cv_ptr->image);
#else
IplImage _ipl_img=cv_ptr->image;
#endif
IplImage *ptr_ipl_img= &_ipl_img;
//For see OpenCV Image:
// Update GUI Window
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
cv::waitKey(3);
this->get_parameter("blue", b);
this->get_parameter("green", g);
this->get_parameter("red", r);
this->get_parameter("seuil", seuil);
CvPoint point;
geometry_msgs::msg::Twist cmd;
point = binarisation(ptr_ipl_img, b, g, r, seuil);
if (point.x != -1 || point.y != -1) {
RCLCPP_INFO(this->get_logger(), "x = %d, y = %d", point.x, point.y);
float cibleY = ptr_ipl_img->height*3/4;
float cmd_x_pixel_value = 2.0 / ptr_ipl_img->width;
float cmd_y_pixel_value = 2.0/(ptr_ipl_img->height - cibleY);
cmd.angular.z = - ((point.x * cmd_x_pixel_value) - 1.0);
cmd.linear.x = - ((point.y - cibleY) * cmd_y_pixel_value);
}
else {
cmd.angular.z = 0.0;
cmd.angular.x = 0.0;
}
pub_->publish(cmd);
}
std::uint8_t b;
std::uint8_t g;
std::uint8_t r;
std::uint8_t seuil;
};
int main(int argc, char ** argv)
{
(void) argc;
(void) argv;
rclcpp::init(argc, argv);
auto node = std::make_shared<Visual>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
Step 6 :
--------
Now let's end this method, it is missing the algorithm for the movement, add it::
if (point.x != -1 || point.y != -1) {
RCLCPP_INFO(this->get_logger(), "x = %d, y = %d", point.x, point.y);
float cibleY = ptr_ipl_img->height*3/4;
float cmd_x_pixel_value = 2.0 / ptr_ipl_img->width;
float cmd_y_pixel_value = 2.0/(ptr_ipl_img->height - cibleY);
cmd.angular.z = - ((point.x * cmd_x_pixel_value) - 1.0);
cmd.linear.x = - ((point.y - cibleY) * cmd_y_pixel_value);
}
else {
cmd.angular.z = 0.0;
cmd.angular.x = 0.0;
}
If the console return -1 as pose, the camera detect no object. Else the console return the pose of the object. Add the last line to publish the method::
pub_->publish(cmd);
We'll now make a chronometer for the node, put this in the begin of the method::
rclcpp::Time begin = get_clock()->now();
This will capture the time at the begin of the method, and put it which will capture the time at the end of the method and will doing the soustraction of both time::
rclcpp::Time end = get_clock()->now();
RCLCPP_INFO(this->get_logger(), "begin : %ld", begin.nanoseconds());
RCLCPP_INFO(this->get_logger(), "end : %ld", end.nanoseconds());
RCLCPP_INFO(this->get_logger(), "duration : %ld", (end.nanoseconds() - begin.nanoseconds()));
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment