Commit b9a306fe authored by CANNERE Xavier's avatar CANNERE Xavier
Browse files

6.1.3

parent 6ef12e68
......@@ -8,7 +8,7 @@ Step 1 : Create the visual follower package
We'll create a new package who allow the following of a visual target called ``pkg_visu`` and a new node ``visu.cpp``::
ros2 pkg create --build-type ament_cmake pkg_visual --dependencies sensor_msgs geometry_msgs rclcpp OpenCV cv_bridge image_transport --node-name visual cnam DetectionCnam_codels
ros2 pkg create --build-type ament_cmake pkg_visual --dependencies sensor_msgs geometry_msgs rclcpp OpenCV cv_bridge image_transport --node-name visual
.. note::
parameters:
......@@ -26,12 +26,43 @@ We'll create a new package who allow the following of a visual target called ``p
Step 2 :
--------
Open on QtCreator and edit visu.cpp. You can delete all what is written. First, include the libraries::
#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"
Now paste this universal main code::
int main(int argc, char ** argv)
{
(void) argc;
(void) argv;
rclcpp::init(argc, argv);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
- init is the initialization
- spin is a loop of ``node`` which is not even declared
- shutdown is the end of execution
Now we'll create a class ``visual``, we'll declare inside this class the main algorithm. Paste that between the ``include`` and the ``main()``::
class Visu : public rclcpp::Node
{
};
You can add the following line in the ``main`` before the ``spin``, it will define which node will be launched::
auto node = std::make_shared<Visual>();
......@@ -65,279 +96,5 @@ Step 2 :
Paste or make sure you have that on your package's files :
visu.cpp::
#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 Visu : public rclcpp::Node
{
public:
Visu()
: 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(&Visu::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();
//necessary for transform ros image type into opencv image type
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<Visu>();
rclcpp::spin(node);
printf("hello world pkg_visu package\n");
rclcpp::shutdown();
return 0;
}
CMakeLists.txt::
cmake_minimum_required(VERSION 3.8)
project(pkg_visu)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(OpenCV REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(image_transport REQUIRED)
add_executable(visu src/visu.cpp src/DetectionCnam_codels.cpp)
target_include_directories(visu PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_features(visu PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
ament_target_dependencies(
visu
"sensor_msgs"
"geometry_msgs"
"rclcpp"
"OpenCV"
"cv_bridge"
"image_transport"
)
install(TARGETS visu
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
package.xml::
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>pkg_visu</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="Xavier.CANNERE@isae-supaero.fr">etdisc</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>OpenCV</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
Now create a new node ``DetectionCnam_codels.cpp`` into your package and paste this code::
#include "DetectionCnam_codels.hpp"
CvPoint gravcenter;
using namespace std;
CvPoint binarisation(IplImage* image, int b, int g, int r, int tolerance) {
int x, y;
IplImage *mask, *bgr;
IplConvKernel *kernel;
int sommeX = 0, sommeY = 0;
int nbPixels = 0;
//gravcenter = {0,0};
gravcenter.x=0;
gravcenter.y=0;
// Create the mask &initialize it to white (no color detected)
mask = cvCreateImage(cvGetSize(image), image->depth, 1);
// Create the bgr image
bgr = cvCloneImage(image);
// We create the mask
cvInRangeS(bgr, cvScalar(b-tolerance, g-tolerance,r-tolerance), cvScalar(b+tolerance, g+tolerance,r+tolerance), mask);
// Create kernels for the morphological operation
kernel = cvCreateStructuringElementEx(5, 5, 2, 2, CV_SHAPE_ELLIPSE);
// Morphological opening (inverse because we have white pixels on black background)
cvDilate(mask, mask, kernel, 1);
cvErode(mask, mask, kernel, 1);
// We go through the mask to look for the tracked object and get its gravity center
for(x = 0; x < mask->width; x++) {
for(y = 0; y < mask->height; y++) {
// If its a tracked pixel, count it to the center of gravity's calcul
if(((uchar *)(mask->imageData + y*mask->widthStep))[x] == 255) {
sommeX += x;
sommeY += y;
(nbPixels)++;
}
}
}
// Show the result of the mask image
cvShowImage("DetectionCnam_Codels Mask", mask);
//Image camera
cvShowImage("image_camera",image);
cvWaitKey(3);
// We release the memory of kernels
cvReleaseStructuringElement(&kernel);
// We release the memory of the mask
cvReleaseImage(&mask);
// We release the memory of the hsv image
cvReleaseImage(&bgr);
//return nbPixels;
if(nbPixels > 0)
{
// gravcenter = ((int)(sommeX / (nbPixels)), (int)(sommeY / (*nbPixels)));
gravcenter.x = (int)(sommeX / (nbPixels));
gravcenter.y = (int)(sommeY / (nbPixels));
cvCircle(image,gravcenter,10,cvScalar(0,0,255,0),2);
cvShowImage("image_camera",image);
cvWaitKey(3);
return gravcenter;
} else
return cvPoint(-1, -1);
}
And build your package.
Step 2 : Make your ROS1 workspace
---------------------------------
We'll create a workspace for ROS1 and clone from a git repository a package that allow to recognize object.
.. warning::
Be careful to source in ROS1 each new terminal (``. /opt/ros/noetic/setup.bash``)
In a new terminal type::
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
cd ..
catkin_make
Step 3 :
------------------------
Install the open_cv library::
sudo -E apt-get install libopencv-dev
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