Commit 79f004b3 authored by Darren Tsai's avatar Darren Tsai
Browse files

added ros callback for img topic functionality

parent 2e52fb22
Pipeline #7815 failed with stage
in 1 minute and 59 seconds
......@@ -73,3 +73,9 @@ ${catkin_LIBRARIES}
${OpenCV_LIBS}
)
## roscb client
add_executable(tuatara_client_roscb src/tuatara_client_roscb.cpp)
target_link_libraries(tuatara_client_roscb
${catkin_LIBRARIES}
${OpenCV_LIBS}
)
<?xml version="1.0" encoding="utf-8"?>
<launch>
<node pkg="tuatara_example" type="tuatara_client_roscb" name="tuatara_example_client" output="screen" required="true" >
<param name="img_topic" value="/gmsl/A0/image_color"/>
</node>
<node pkg="tuatara" type="tuatara_server" name="tuatara_server" output="screen" required="true" >
<param name="~model_name" value="yolo4_fp32"/>
<param name="~n_max_batch" value="1"/>
<remap from="~out/image_with_bboxes" to="image_with_bboxes"/>
</node>
</launch>
......@@ -70,6 +70,7 @@ int main (int argc, char **argv)
int th = 0;
int iw = 0;
int ih = 0;
switch( input_videos_list.size() ) {
case 1: tw = th = 1; iw = 1280; ih = 720; break;
case 2: tw = 2; th = 1; iw = 640; ih = 360; break;
......@@ -155,7 +156,7 @@ int main (int argc, char **argv)
}
else
ROS_ERROR_STREAM(ros::this_node::getName() << ": Action did not finish before the time out.");
cv::Mat combined = cv::Mat(th * ih, tw * iw, batch_frame[0].type());
//std::cout << th * ih << "," << tw * iw << std::endl;
for( int i = 0; i < th; i++ ) {
......
#include <chrono> // for high_resolution_clock
#include <ros/ros.h>
#include <ros/package.h>
#include <sensor_msgs/Image.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <tuatara_msgs/GenerateBoundingBoxesAction.h>
#include <boost/filesystem.hpp>
int seq = 0;
actionlib::SimpleActionClient< tuatara_msgs::GenerateBoundingBoxesAction > *acPtr_;
void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
ROS_INFO("Image callback invoked");
int tw = 1;
int th = 1;
int iw = 1280;
int ih = 720;
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
tuatara_msgs::GenerateBoundingBoxesGoal goal;
seq++;
goal.images.clear();
goal.id = seq;
goal.images.push_back(*img_msg);
// Record start time
auto t_start = std::chrono::high_resolution_clock::now();
// send a goal to the action
acPtr_->sendGoal(goal);
//wait for the action to return
bool finished_before_timeout = acPtr_->waitForResult(ros::Duration(30.0));
// Record end time
auto t_finish = std::chrono::high_resolution_clock::now();
std::chrono::duration< double > t_elapsed = t_finish - t_start;
ROS_INFO_STREAM(ros::this_node::getName() << ": Process time " << 1000.0f * t_elapsed.count() << " ms");
if (finished_before_timeout) {
actionlib::SimpleClientGoalState state = acPtr_->getState();
ROS_INFO_STREAM(ros::this_node::getName() << ": Action finished: " << state.toString());
tuatara_msgs::GenerateBoundingBoxesResultConstPtr result = acPtr_->getResult();
for( int bi = 0; bi < result->results.size(); bi++ ) {
auto bboxes = result->results[bi].bounding_boxes;
cv::Scalar class_color = CV_RGB(80, 80, 80);
int nCars = 0, nPeds = 0;
for(auto bbox : bboxes) {
//ROS_INFO_STREAM(bbox.Class);
if(bbox.Class == "car") {
class_color = CV_RGB(255, 0, 0);
nCars++;
}
else if(bbox.Class == "person"){
class_color = CV_RGB(0, 255, 0);
nPeds++;
}
cv::rectangle(cv_ptr->image, cv::Point(bbox.xmin, bbox.ymin), cv::Point(bbox.xmax, bbox.ymax), class_color, 2);
}
ROS_INFO_STREAM(ros::this_node::getName() << ": Detected " << nCars << " car(s) and " << nPeds << " pedestrian(s)");
}
}
else
ROS_ERROR_STREAM(ros::this_node::getName() << ": Action did not finish before the time out.");
}
int main (int argc, char **argv)
{
ros::init(argc, argv, "tuatara_client_roscb");
ros::NodeHandle nh("~");
std::string img_topic;
nh.getParam("img_topic", img_topic);
ros::Subscriber img_sub = nh.subscribe(img_topic, 20, img_callback);
// Currently the action server is publishing image with bboxes so I'm leaving this out
// ros::Publisher image_with_bboxes_pub = nh.advertise< sensor_msgs::Image >(img_topic + "/image_with_bboxes", 1);
// create the action client
// true causes the client to spin its own thread
acPtr_ = new actionlib::SimpleActionClient< tuatara_msgs::GenerateBoundingBoxesAction >("/tuatara_server/generate_bounding_boxes", true);
ROS_INFO_STREAM(ros::this_node::getName() << ": Waiting for tuatara action server " << "/tuatara_server/generate_bounding_boxes" << " to start.");
// wait for the action server to start
acPtr_->waitForServer(); //will wait for infinite time
ROS_INFO_STREAM(ros::this_node::getName() << ": tuatara action server is running");
ros::spin();
return 0;
}
Supports Markdown
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