Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Mod road wizard #22

Merged
merged 2 commits into from
Sep 9, 2015
Merged

Mod road wizard #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
313096e
Modified feat_proj so that we can adjust projection error from runtim…
Sep 9, 2015
503ff41
Modified region_tlr so that we can switch displaying image of superim…
Sep 9, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions ros/src/computing/perception/detection/packages/road_wizard/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@ target_link_libraries(feat_proj
)

add_dependencies(feat_proj
runtime_manager_generate_messages_cpp
road_wizard_generate_messages_cpp
)

Expand Down
23 changes: 15 additions & 8 deletions ros/src/computing/perception/detection/packages/road_wizard/nodes/feat_proj/feat_proj.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,13 @@
#include "Math.h"
#include <Eigen/Eigen>
#include "road_wizard/Signals.h"
#include <runtime_manager/adjust_xy.h>

static constexpr uint32_t SUBSCRIBE_QUEUE_SIZE = 1000;

static int adjust_proj_x = 0;
static int adjust_proj_y = 0;

typedef struct {
double thiX;
double thiY;
Expand All @@ -44,6 +48,12 @@ static tf::StampedTransform trf;

#define SignalLampRadius 0.3

/* Callback function to shift projection result */
void adjust_xyCallback (const runtime_manager::adjust_xy::ConstPtr& config_msg)
{
adjust_proj_x = config_msg->x;
adjust_proj_y = config_msg->y;
}

void cameraInfoCallback (const sensor_msgs::CameraInfo::ConstPtr camInfoMsg)
{
Expand Down Expand Up @@ -190,14 +200,10 @@ void echoSignals2 (ros::Publisher &pub, bool useOpenGLCoord=false)

road_wizard::ExtractedPosition sign;
sign.signalId = signal.id;
//sign.u = u, sign.v = v, sign.radius = radius;
/* if ndt's Angle Error == 0 */
sign.u = u;
sign.v = v - 25; /* Temporary correction of calibration data */

/* if ndt's Angle Error == 1.5 */
// sign.u = u + 35;
// sign.v = v - 28; /* Temporary correction of calibration data */

sign.u = u + adjust_proj_x; // shift project position by configuration value from runtime manager
sign.v = v + adjust_proj_y; // shift project position by configuration value from runtime manager

sign.radius = radius;
sign.x = signalcenter.x(), sign.y = signalcenter.y(), sign.z = signalcenter.z();
sign.hang = vmap.vectors[signal.vid].hang; // hang is expressed in [0, 360] degree
Expand Down Expand Up @@ -290,6 +296,7 @@ int main (int argc, char *argv[])
std::cout << "all vector map loaded." << std::endl;

ros::Subscriber cameraInfoSubscriber = rosnode.subscribe ("/camera/camera_info", 100, cameraInfoCallback);
ros::Subscriber adjust_xySubscriber = rosnode.subscribe("/config/adjust_xy", 100, adjust_xyCallback);
// ros::Subscriber ndtPoseSubscriber = rosnode.subscribe("/current_pose", 10, ndtPoseCallback);
ros::Publisher signalPublisher = rosnode.advertise <road_wizard::Signals> ("roi_signal", 100);
signal (SIGINT, interrupt);
Expand Down
65 changes: 42 additions & 23 deletions ros/src/computing/perception/detection/packages/road_wizard/nodes/region_tlr/region_tlr.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#include "road_wizard/TunedResult.h"
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <std_msgs/Bool.h>

thresholdSet thSet;

Expand All @@ -27,6 +28,8 @@ static TrafficLightDetector detector;

static cv::Mat frame;

static bool show_superimpose_result = false;
static const std::string window_name = "superimpose result";

static double cvtInt2Double_hue(int center, int range)
{
Expand Down Expand Up @@ -131,6 +134,28 @@ static void image_raw_cb(const sensor_msgs::Image& image_source)
// cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(image_source);
frame = cv_image->image.clone();

if (show_superimpose_result)
{
/* display superimpose result */
cv::Mat targetScope = frame.clone();
for (unsigned int i=0; i<detector.contexts.size(); i++)
{
/* draw superimposed position of traffic lights */
circle(targetScope, detector.contexts.at(i).redCenter, detector.contexts.at(i).lampRadius, CV_RGB(255, 0, 0), 1, 0);
circle(targetScope, detector.contexts.at(i).yellowCenter, detector.contexts.at(i).lampRadius, CV_RGB(255, 255, 0), 1, 0);
circle(targetScope, detector.contexts.at(i).greenCenter, detector.contexts.at(i).lampRadius, CV_RGB(0, 255, 0), 1, 0);
}

/* draw detection results */
putResult_inText(&targetScope, detector.contexts);

if (cvGetWindowHandle(window_name.c_str()) != NULL) // Guard not to write destroyed window by using close button on the window
{
imshow(window_name, targetScope);
cv::waitKey(5);
}
}

} /* static void image_raw_cb() */


Expand All @@ -141,31 +166,8 @@ static void extractedPos_cb(const road_wizard::Signals::ConstPtr& extractedPos)

setContexts(detector, extractedPos);

/* test output */
cv::Mat targetScope;
frame.copyTo(targetScope);
// for (unsigned int i=0; i<detector.contexts.size(); i++)
// {
// // if (detector.contexts.at(i).lampRadius < MINIMAM_RADIUS)
// // continue;

// circle(targetScope, detector.contexts.at(i).redCenter, detector.contexts.at(i).lampRadius, CV_RGB(255, 0, 0), 1, 0);
// circle(targetScope, detector.contexts.at(i).yellowCenter, detector.contexts.at(i).lampRadius, CV_RGB(255, 255, 0), 1, 0);
// circle(targetScope, detector.contexts.at(i).greenCenter, detector.contexts.at(i).lampRadius, CV_RGB(0, 255, 0), 1, 0);
// }


// cv::Mat grayScale;
// cvtColor(frame, grayScale, CV_RGB2GRAY);
// detector.brightnessDetect(grayScale);
detector.brightnessDetect(frame);

/* test output */
putResult_inText(&targetScope, detector.contexts);

// imshow("detection result", targetScope);
// waitKey(5);

/* publish result */
runtime_manager::traffic_light state_msg;
std_msgs::String state_string_msg;
Expand Down Expand Up @@ -351,6 +353,22 @@ static void tunedResult_cb(const road_wizard::TunedResult& msg)
} /* static void tunedResult_cb() */


static void superimpose_cb(const std_msgs::Bool::ConstPtr& config_msg)
{
show_superimpose_result = config_msg->data;

if (show_superimpose_result) {
cv::namedWindow(window_name, cv::WINDOW_NORMAL);
cv::startWindowThread();
}

if (!show_superimpose_result) {
cv::destroyWindow(window_name);
cv::waitKey(1);
}

} /* static void superimpose_cb() */

int main(int argc, char* argv[]) {

// printf("***** Traffic lights app *****\n");
Expand Down Expand Up @@ -384,6 +402,7 @@ int main(int argc, char* argv[]) {
ros::Subscriber image_sub = n.subscribe("/image_raw", 1, image_raw_cb);
ros::Subscriber position_sub = n.subscribe("/roi_signal", 1, extractedPos_cb);
ros::Subscriber tunedResult_sub = n.subscribe("/tuned_result", 1, tunedResult_cb);
ros::Subscriber superimpose_sub = n.subscribe("/config/superimpose", 1, superimpose_cb);

signalState_pub = n.advertise<runtime_manager::traffic_light>("/light_color", ADVERTISE_QUEUE_SIZE, ADVERTISE_LATCH);
signalStateString_pub = n.advertise<std_msgs::String>("/sound_player", ADVERTISE_QUEUE_SIZE);
Expand Down