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

Camera frames integrated #51

Merged
merged 4 commits into from
Oct 1, 2019
Merged

Camera frames integrated #51

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
fb7f30c
Camera frames integrated
Snehal-Reddy Apr 6, 2019
ff05723
One ball sent
Snehal-Reddy Apr 7, 2019
451543f
bug fixes for vision node
hardik-kgp Sep 5, 2019
6abedf6
Merge pull request #3 from hardik-kgp/patch-1
Snehal-Reddy Sep 7, 2019
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
146 changes: 112 additions & 34 deletions vision_comm/src/vision_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
#include "ros/ros.h"
#include "std_msgs/String.h"

#include <bits/stdc++.h>
#include "robocup_ssl_client.h"
#include <sstream>
#include <krssg_ssl_msgs/SSL_DetectionFrame.h>
Expand All @@ -10,7 +10,9 @@
#include "messages_robocup_ssl_wrapper.pb.h"
#include <ctime>
#include "timer.h"
#include <unistd.h>

#define num_cam 4

using namespace std;

Expand All @@ -30,18 +32,28 @@ int main(int argc, char **argv)
ros::Publisher chatter_pub = n.advertise<krssg_ssl_msgs::SSL_DetectionFrame>("vision", 10000);
// ros::Rate loop_rate(10);


float max_ball_confidence = 0;
float max_blue_bot_confidence[6] = {0};
float max_yellow_bot_confidence[6] = {0};
int camera_bool[num_cam] = {0};
GOOGLE_PROTOBUF_VERIFY_VERSION;
int port = use_grsim_vision? 10020: 10016;
string net_address = "224.5.23.1";
int port = use_grsim_vision? 10020: 10006;
string net_address = "224.5.23.2";
RoboCupSSLClient client(port, net_address);
client.open();
printf("Connected to %s.\n", use_grsim_vision? "grSim vision" : "ssl-vision");
SSL_WrapperPacket packet;
krssg_ssl_msgs::SSL_DetectionFrame msg;
int is_blue_bot_detected[6] = {0};
int is_yellow_bot_detected[6] = {0};
map< int, krssg_ssl_msgs::SSL_DetectionRobot > blue_bots;
map< int, krssg_ssl_msgs::SSL_DetectionRobot > yellow_bots;
krssg_ssl_msgs::SSL_DetectionBall ball;
while(ros::ok()) {
printf("........\n");
//printf("........abcd\n");
if (client.receive(packet)) {
krssg_ssl_msgs::SSL_DetectionFrame msg;
cout<<"In IF************\n";

//see if the packet contains a robot detection frame:
if (packet.has_detection()) {
SSL_DetectionFrame detection = packet.detection();
Expand All @@ -51,56 +63,121 @@ int main(int argc, char **argv)

msg.frame_number = detection.frame_number();
msg.t_capture = detection.t_capture();

cout<<"Frame number: "<<msg.frame_number<<endl;
msg.t_sent = detection.t_sent();
msg.camera_id = detection.camera_id();
//Ball info:
cout<<"Camera num: "<<detection.camera_id()<<endl;
if (camera_bool[detection.camera_id()] == 1)
{
max_ball_confidence = 0;
msg.balls.push_back(ball);
// cout << "ball.x"
}
for (int i = 0; i < balls_n; i++) {
SSL_DetectionBall ball = detection.balls(i);
krssg_ssl_msgs::SSL_DetectionBall ballmsg;
ballmsg.confidence = ball.confidence();
ballmsg.area = ball.area();
ballmsg.x = ball.x();
ballmsg.y = ball.y();
ballmsg.z = ball.z();
SSL_DetectionBall ball_new = detection.balls(i);

//krssg_ssl_msgs::SSL_DetectionBall ballmsg;
if (ball_new.confidence() > max_ball_confidence)
{
max_ball_confidence = ball_new.confidence();
ball.confidence = ball_new.confidence();
ball.area = ball_new.area();
ball.x = ball_new.x();
ball.y = ball_new.y();
ball.z = ball_new.z();
}

// Not using
// ballmsg.pixel_x = ball.pixel_x();
// ballmsg.pixel_y = ball.pixel_y();
msg.balls.push_back(ballmsg);
cout << "Ball pos: " << ballmsg.x<<"," << ballmsg.y<<endl;
//msg.balls.push_back(ballmsg);
//cout << "Ball pos: " << ballmsg.x<<"," << ballmsg.y<<endl;
}

//Blue robot info:
//cout<<"********* "<<robots_blue_n<<endl;
if (camera_bool[detection.camera_id()] == 1)
{
//cout<<"Updating Blue bots"<<endl;
for(int i = 0; i < 6; i++)
{
//if(is_blue_bot_detected[i] == 1)
msg.robots_blue.push_back(blue_bots[i]);
max_blue_bot_confidence[i] = 0;
}
for(int i = 0; i < 6; i++)
is_blue_bot_detected[i] = 0;
}
for (int i = 0; i < robots_blue_n; i++) {
SSL_DetectionRobot robot = detection.robots_blue(i);
krssg_ssl_msgs::SSL_DetectionRobot botmsg;
botmsg.confidence = robot.confidence();
botmsg.robot_id = robot.robot_id();
botmsg.x = robot.x();
botmsg.y = robot.y();
botmsg.orientation = robot.orientation();
// krssg_ssl_msgs::SSL_DetectionRobot botmsg;
blue_bots[robot.robot_id()].robot_id = robot.robot_id();
if (robot.confidence() > max_blue_bot_confidence[blue_bots[robot.robot_id()].robot_id])
{
max_blue_bot_confidence[blue_bots[robot.robot_id()].robot_id] = robot.confidence();
blue_bots[robot.robot_id()].confidence = robot.confidence();
blue_bots[robot.robot_id()].x = robot.x();
blue_bots[robot.robot_id()].y = robot.y();
blue_bots[robot.robot_id()].orientation = robot.orientation();
}
// Not using
// botmsg.pixel_x = robot.pixel_x();
// botmsg.pixel_y = robot.pixel_y();
// botmsg.height = robot.height();
msg.robots_blue.push_back(botmsg);
cout << "Blue bot detected, bot_id = " << robot.robot_id() << " pos: " << robot.x() << " " << robot.y() << endl;
//msg.robots_blue.push_back(botmsg);
//cout << "Blue bot detected, bot_id = " << robot.robot_id() << " pos: " << robot.x() << " " << robot.y() << endl;
}
if (camera_bool[detection.camera_id()] == 1)
{
for(int i = 0; i < 6; i++)
max_yellow_bot_confidence[i] = 0,is_yellow_bot_detected[i] = 0;
//msg.robots_yellow = yellow_bots;
}

//Yellow robot info:
for (int i = 0; i < robots_yellow_n; i++) {
SSL_DetectionRobot robot = detection.robots_yellow(i);
krssg_ssl_msgs::SSL_DetectionRobot botmsg;
botmsg.confidence = robot.confidence();
botmsg.robot_id = robot.robot_id();
botmsg.x = robot.x();
botmsg.y = robot.y();
botmsg.orientation = robot.orientation();

//krssg_ssl_msgs::SSL_DetectionRobot botmsg;
yellow_bots[robot.robot_id()].robot_id = robot.robot_id();
if (robot.confidence() > max_yellow_bot_confidence[yellow_bots[robot.robot_id()].robot_id])
{
max_yellow_bot_confidence[yellow_bots[robot.robot_id()].robot_id] = robot.confidence();
yellow_bots[robot.robot_id()].confidence = robot.confidence();
yellow_bots[robot.robot_id()].x = robot.x();
yellow_bots[robot.robot_id()].y = robot.y();
yellow_bots[robot.robot_id()].orientation = robot.orientation();
}

// Not using
// botmsg.pixel_x = robot.pixel_x();
// botmsg.pixel_y = robot.pixel_y();
// botmsg.height = robot.height();
msg.robots_yellow.push_back(botmsg);
//msg.robots_yellow.push_back(botmsg);

}
if(camera_bool[detection.camera_id()] == 1)
{
for(int i = 0; i < 6; i++)
{
//if(is_yellow_bot_detected[i] == 1)
msg.robots_yellow.push_back(yellow_bots[i]);

}

cout << "ball size "<<msg.balls.size() << endl;
chatter_pub.publish(msg);
for(int i = 0; i < num_cam; i++)
{
camera_bool[i] = 0;
}
msg.robots_blue.clear();
msg.robots_yellow.clear();
msg.balls.clear();

}
camera_bool[detection.camera_id()] = 1;

}

Expand All @@ -114,12 +191,13 @@ int main(int argc, char **argv)
// const SSL_GeometryCameraCalibration & calib = geom.calib(i);
// }
// }
// printf("sending packet...\n");
chatter_pub.publish(msg);
//cout<<"sending packet...\n";

//chatter_pub.publish(msg);
}
ros::spinOnce();
// loop_rate.sleep();
}

return 0;
}
}