Outils pour utilisateurs

Outils du site


synchro_camera

Ceci est une ancienne révision du document !


Noeud pour l'enregistrement des images au format PPM

La camera de gauche est master, elle envoie un signal à la caméra de droite et à elle même pour démarrer la prise de photo. L'arduino récupère se signal pour incrémenter un compteur qu'il affiche sur l'afficheur 8 segments.

Il n'y a pas de launch. Pour lancer le programme:

rosrun test_ueye listenerUeye

adapter les 2 lignes suivantes pour chercher le bon dossier

include_directories( /home/jcombier/DiversTools/OpenCV/opencv-2.4.11/installation/include)
link_directories( /home/jcombier/DiversTools/OpenCV/opencv-2.4.11/installation/lib)
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(test_ueye)
 
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  camera_calibration_parsers
  camera_info_manager
  image_transport
  nodelet
  roscpp
  sensor_msgs
  std_msgs
  cv_bridge
)
#find_package(OpenCV REQUIRED) # Have to be in a different find_package() line
 
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
 
 
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
 
 
################################################
## Declare ROS messages, services and actions ##
################################################
 
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )
 
## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )
 
## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )
 
## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   sensor_msgs#   std_msgs
# )
 
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
 
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed
 
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )
 
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
  INCLUDE_DIRS include
  LIBRARIES test_ueye
  CATKIN_DEPENDS camera_calibration_parsers camera_info_manager image_transport nodelet roscpp sensor_msgs std_msgs cv_bridge
#  DEPENDS system_lib
)
 
###########
## Build ##
###########
 
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
  ${catkin_INCLUDE_DIRS}
#  ${OpenCV_INCLUDE_DIRS}
)
#--- FOR LOCAL OPENCV COMPILED LIBRAIRIE ---
include_directories( /home/jcombier/DiversTools/OpenCV/opencv-2.4.11/installation/include)
link_directories( /home/jcombier/DiversTools/OpenCV/opencv-2.4.11/installation/lib)
 
## Declare a C++ library
# add_library(test_ueye
#   src/${PROJECT_NAME}/test_ueye.cpp
# )
 
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(test_ueye ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
## Declare a C++ executable
# add_executable(test_ueye_node src/test_ueye_node.cpp)
 
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(test_ueye_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
## Specify libraries to link a library or executable target against
# target_link_libraries(test_ueye_node
#   ${catkin_LIBRARIES}
# )
 
#--- FOR LOCAL OPENCV COMPILED LIBRAIRIE ---
set(OpenCV_LIBS opencv_core opencv_imgproc opencv_calib3d opencv_imgcodecs opencv_video opencv_features2d opencv_ml opencv_highgui opencv_objdetect opencv_contrib opencv_legacy opencv_gpu)
 
add_executable(listenerUeye src/test_ueye.cpp)
target_link_libraries(listenerUeye ${catkin_LIBRARIES})
target_link_libraries(listenerUeye ${OpenCV_LIBS}) #/home/jcombier/DiversTools/OpenCV/opencv-2.4.11/cmake/
add_dependencies(listenerUeye test_ueye_cpp)
 
#############
## Install ##
#############
 
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
 
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
 
## Mark executables and/or libraries for installation
# install(TARGETS test_ueye test_ueye_node
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
 
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )
 
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
 
#############
## Testing ##
#############
 
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_test_ueye.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
 
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
test_ueye.cpp
// from ueyecam_nodelet
#include <ros/package.h>
#include <camera_calibration_parsers/parse.h>
#include <sensor_msgs/fill_image.h>
#include <sensor_msgs/image_encodings.h>
 
#include <sstream>
#include "std_msgs/String.h"
 
// code sample image transport
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
 
// For time synchronizer
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
 
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <cstring>
 
using namespace sensor_msgs;
using namespace message_filters;
 
unsigned int k(0);
 
/*****************************************************/
/***                  SAVE_PPM                     ***/
/*****************************************************/
void save_ppm(char *filename, char *im, unsigned int width, unsigned int height)
{
    FILE *f;
    if(!(f=fopen(filename,"wb"))) {
        fprintf(stderr,"Could not open `%s'",filename);
        exit(1);
    }
 
    fprintf(f,"P6\n%d %d\n255\n",width,height);
    fwrite(im,1,width*height*3,f);
    fclose(f);
}
 
/*****************************************************/
/***              IMAGE CALLBACK                   ***/
/*****************************************************/
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
        cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
        cv::waitKey(30);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}
 
/*****************************************************/
/***              STEREO CALLBACK                  ***/
/*****************************************************/
void stereoCallback(const ImageConstPtr& image1, const ImageConstPtr& image2)
{
    // Solve all of perception here...
    try
    {
        cv_bridge::CvImagePtr cv_ptr_1 = cv_bridge::toCvCopy(image1, "bgr8");
        cv_bridge::CvImagePtr cv_ptr_2 = cv_bridge::toCvCopy(image2, "bgr8");
 
        // save left image
        std::stringstream sstm;
        std::string nameFileL;
        sstm << "/tmp/images/ueye_" << std::setfill('0') << std::setw(5) << k << "_left" << ".ppm" ;
        nameFileL = sstm.str();
        save_ppm((char*)nameFileL.c_str(), (char*)cv_ptr_1->image.data, cv_ptr_1->image.cols, cv_ptr_1->image.rows);
 
        // save right image
        std::string nameFileR;
        sstm.str(std::string());
        sstm << "/tmp/images/ueye_" << std::setfill('0') << std::setw(5) << k << "_right" << ".ppm" ;
        nameFileR = sstm.str();
        save_ppm((char*)nameFileR.c_str(), (char*)cv_ptr_2->image.data, cv_ptr_2->image.cols, cv_ptr_2->image.rows);
 
        // show image
        cv::imshow("view_left", cv_ptr_1->image);
        cv::imshow("view_right", cv_ptr_2->image);
        cv::waitKey(30);
        k++;
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", image1->encoding.c_str());
    }
}
 
/*****************************************************/
/***                    MAIN                       ***/
/*****************************************************/
int main(int argc, char **argv)
{
    // create folder where we save images
    system("mkdir /tmp/images");
 
    ros::init(argc, argv, "listenerUeye");
    ros::NodeHandle nh;
    bool stereo(true);
 
    if(!stereo){
        cv::namedWindow("view");
        cv::startWindowThread();
 
        image_transport::ImageTransport it(nh);
        image_transport::Subscriber sub = it.subscribe("/stereo/left/image_raw", 1, imageCallback);
 
        ros::spin();
 
        cv::destroyWindow("view");
    }
    else{
        cv::namedWindow("view_left");
        cv::namedWindow("view_right");
        cv::startWindowThread();
 
        message_filters::Subscriber<Image> image_left_sub(nh, "/stereo/left/image_raw", 1);
        message_filters::Subscriber<Image> image_right_sub(nh, "/stereo/right/image_raw", 1);
        TimeSynchronizer<Image, Image> sync(image_left_sub, image_right_sub, 10);
        sync.registerCallback(boost::bind(&stereoCallback, _1, _2));
 
        ros::spin();
 
        cv::destroyWindow("view_left");
        cv::destroyWindow("view_right");
    }
}

Résultats

synchro_camera.1478265914.txt.gz · Dernière modification : 2016/11/04 14:25 de bvandepo