Main Page
Namespaces
Classes
Files
File List
File Members
src
perception
road
caltech_lanes
src
main.cpp
Go to the documentation of this file.
1
/**************************************************************************************************
2
Software License Agreement (BSD License)
3
4
Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5
All rights reserved.
6
7
Redistribution and use in source and binary forms, with or without modification, are permitted
8
provided that the following conditions are met:
9
10
*Redistributions of source code must retain the above copyright notice, this list of
11
conditions and the following disclaimer.
12
*Redistributions in binary form must reproduce the above copyright notice, this list of
13
conditions and the following disclaimer in the documentation and/or other materials provided
14
with the distribution.
15
*Neither the name of the University of Aveiro nor the names of its contributors may be used to
16
endorse or promote products derived from this software without specific prior written permission.
17
18
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20
FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24
IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26
***************************************************************************************************/
33
#include <ros/ros.h>
34
#include <image_transport/image_transport.h>
35
#include <cv_bridge/cv_bridge.h>
36
#include <sensor_msgs/image_encodings.h>
37
#include <opencv2/imgproc/imgproc.hpp>
38
#include <opencv2/highgui/highgui.hpp>
39
#include <stdio.h>
40
#include <stdlib.h>
41
#include <signal.h>
42
#include "
ProcessImage.h
"
43
44
#include "
main.hh
"
45
#include "
cmdline.h
"
46
#include "
LaneDetector.hh
"
47
#include <stdio.h>
48
#include <fstream>
49
#include <sstream>
50
#include <iostream>
51
#include <iomanip>
52
#include <ctime>
53
54
55
namespace
enc = sensor_msgs::image_encodings;
56
57
// Useful message macro
58
#define MSG(fmt, ...) \
59
(fprintf(stdout, "%s:%d msg " fmt "\n", __FILE__, __LINE__, ##__VA_ARGS__) ? 0 : 0)
60
61
// Useful error macro
62
#define ERROR(fmt, ...) \
63
(fprintf(stderr, "%s:%d error " fmt "\n", __FILE__, __LINE__, ##__VA_ARGS__) ? -1 : -1)
64
using namespace
std;
65
using namespace
cv;
66
67
68
Procecess
pross
;
69
73
void
sighandler
(
int
sig)
74
{
75
ROS_ERROR(
"Signal %d caught..."
,sig);
76
cout<<
"Shuting down road_recognition"
<<endl;
77
exit(0);
78
}
79
80
81
namespace
LaneDetector
82
{
86
void
imageCallback
(
const
sensor_msgs::ImageConstPtr& original_image)
87
{
88
89
cv_bridge::CvImagePtr cv_ptr;
90
91
try
92
{
93
cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
94
}
catch
(cv_bridge::Exception& e)
95
{
96
ROS_ERROR(
"Caltech::main.cpp::cv_bridge exception:%s"
,e.what());
97
exit(0);
98
}
99
100
101
// elapsed time
102
clock_t elapsed = 0;
103
104
pross
.
ProcessImage
( cv_ptr->image, NULL, 0,&elapsed);
105
double
elapsedTime =
static_cast<
double
>
(elapsed) / CLOCKS_PER_SEC;
106
MSG
(
"Total time %f secs for 1 image = %f Hz"
, elapsedTime,
107
1. / elapsedTime);
108
cv::waitKey(10);
109
}
110
}
114
int
main
(
int
argc,
char
** argv)
115
{
116
117
ros::init(argc, argv,
"caltech_lane"
);
118
// carregar as definições para a class
119
pross
.
Load_config
( argc , argv );
120
121
ros::NodeHandle nh;
122
signal(SIGINT, &
sighandler
);
123
124
/* load all the paramters of the camera */
125
// pross.Load_config(argc, argv);
126
127
/* Create an ImageTransport instance, initializing it with our NodeHandle. */
128
image_transport::ImageTransport it(nh);
129
image_transport::Subscriber sub = it.subscribe(
"image"
, 1,
imageCallback
);
130
//
131
//
132
ros::spin();
133
ROS_INFO(
"Caltech::main.cpp::No error."
);
134
}
pross
Procecess pross
Definition:
main.cpp:68
main
int main(int argc, char **argv)
Definition:
main.cpp:114
MSG
#define MSG(fmt,...)
Definition:
main.cpp:58
LaneDetector::imageCallback
void imageCallback(const sensor_msgs::ImageConstPtr &original_image)
Definition:
main.cpp:86
main.hh
ProcessImage.h
Class para fazer o processamento da imagem.
sighandler
void sighandler(int sig)
Definition:
main.cpp:73
Procecess
Definition:
ProcessImage.h:70
LaneDetector.hh
cmdline.h
Procecess::Load_config
int Load_config(int argc, char **argv)
Definition:
ProcessImage.h:83
Procecess::ProcessImage
void ProcessImage(Mat &raw_mat, ofstream *outputFile, int index, clock_t *elapsedTime)
Definition:
ProcessImage.h:124
caltech_lanes
Author(s): Ricardo Morais
autogenerated on Mon Mar 2 2015 01:31:31