Commit 8102c371 authored by CANNERE Xavier's avatar CANNERE Xavier
Browse files

t6 final

parent 9d02516b
......@@ -197,8 +197,8 @@ After that you can put on comment the *RCLCPP* lines, there were here to test th
return 0;
}
Step 4 : Complete the methods
-----------------------------
Step 4 : Analyze the image
--------------------------
In *getImage* complete the code with that. Depending the version of OpenCv, it will choose different configuration::
......@@ -415,8 +415,8 @@ Your main files should look like this::
return 0;
}
Step 5 :
--------
Step 5 : Parameters
-------------------
Add this following code. We are declaring a point in the image and a message which contains pose::
......@@ -541,8 +541,8 @@ You should have this code::
return 0;
}
Step 6 :
--------
Step 6 : Calculate the distance and the time of execution
---------------------------------------------------------
Now let's end this method, it is missing the algorithm for the movement, add it::
......@@ -574,8 +574,108 @@ This will capture the time at the begin of the method, and put it which will cap
RCLCPP_INFO(this->get_logger(), "end : %ld", end.nanoseconds());
RCLCPP_INFO(this->get_logger(), "duration : %ld", (end.nanoseconds() - begin.nanoseconds()));
You should have this final 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)
{
rclcpp::Time begin = get_clock()->now();
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);
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()));
}
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;
}
We'll operate this node in the next chapter.
......
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