120 lines
3.7 KiB
C++
Executable File
120 lines
3.7 KiB
C++
Executable File
/*
|
|
* Copyright (C) 2021, Yubao Liu, AISL, TOYOHASHI UNIVERSITY of TECHNOLOGY
|
|
* Email: yubao.liu.ra@tut.jp
|
|
*
|
|
*/
|
|
|
|
#include <segnet_ros/segnetAction.h>
|
|
|
|
#include <rosbag/bag.h>
|
|
#include <rosbag/view.h>
|
|
|
|
#include <actionlib/client/simple_action_client.h>
|
|
|
|
using namespace std;
|
|
// using namespace segnet_ros;
|
|
|
|
typedef actionlib::SimpleActionClient<segnet_ros::segnetAction> Client;
|
|
|
|
namespace segnet_ros {
|
|
|
|
class SegNetClient {
|
|
public:
|
|
typedef std::shared_ptr<SegNetClient> Ptr;
|
|
segnetResultConstPtr mResult;
|
|
|
|
SegNetClient(const std::string t_name, const bool t_spin)
|
|
: ac_(t_name, t_spin)
|
|
{
|
|
ROS_INFO("Waiting for action server to start.");
|
|
std::cout << "Waiting for action server to start." << std::endl;
|
|
ac_.waitForServer();
|
|
ROS_INFO("Action server started, sending goal.");
|
|
factory_id_ = -1;
|
|
}
|
|
|
|
void Segment(const std::vector<cv::Mat>& in_images, std::vector<cv::Mat>& out_label)
|
|
{
|
|
segnetGoal goal;
|
|
cv_bridge::CvImageConstPtr cvpLabel;
|
|
int batch_size = in_images.size();
|
|
if (batch_size <= 0) {
|
|
LOG(ERROR) << "No image data";
|
|
return;
|
|
}
|
|
for (size_t i = 0; i < batch_size; i++) {
|
|
cv_bridge::CvImage cvImage(std_msgs::Header(), sensor_msgs::image_encodings::BGR8, in_images[i]);
|
|
goal.image = *(cvImage.toImageMsg());
|
|
goal.id = ++factory_id_;
|
|
|
|
ac_.sendGoal(goal);
|
|
|
|
std::cout << "Request ID: " << goal.id << std::endl;
|
|
LOG(INFO) << "Request ID: " << goal.id;
|
|
|
|
//wait for the action to return
|
|
bool finished_before_timeout = ac_.waitForResult(ros::Duration(30.0));
|
|
|
|
if (finished_before_timeout) {
|
|
actionlib::SimpleClientGoalState state = ac_.getState();
|
|
ROS_INFO("Action finished: %s", state.toString().c_str());
|
|
} else {
|
|
ROS_INFO("Action did not finish before the time out.");
|
|
}
|
|
|
|
mResult = ac_.getResult();
|
|
cv::Mat label = cv_bridge::toCvCopy(mResult->label)->image;
|
|
out_label.push_back(label);
|
|
} // end for batch
|
|
}
|
|
|
|
void doneCb(const actionlib::SimpleClientGoalState& state,
|
|
const segnetResultConstPtr& result)
|
|
{
|
|
ROS_INFO("Finished in state [%s]", state.toString().c_str());
|
|
ROS_INFO("Answer: %i", result->id);
|
|
|
|
cv_bridge::CvImageConstPtr cvpLabel, cvpLabelColor;
|
|
try {
|
|
cvpLabel = cv_bridge::toCvCopy(result->label);
|
|
cvpLabelColor = cv_bridge::toCvCopy(result->label_color);
|
|
} catch (cv_bridge::Exception& e) {
|
|
ROS_ERROR("cv_bridge exception: %s", e.what());
|
|
}
|
|
|
|
cv::imshow("label", cvpLabelColor->image);
|
|
cv::waitKey(1);
|
|
}
|
|
|
|
void doStuff(const sensor_msgs::ImageConstPtr imgMsg)
|
|
{
|
|
segnetGoal goal;
|
|
goal.id = ++factory_id_;
|
|
goal.image = *imgMsg;
|
|
|
|
// Need boost::bind to pass in the 'this' pointer
|
|
ac_.sendGoal(goal,
|
|
boost::bind(&SegNetClient::doneCb, this, _1, _2),
|
|
Client::SimpleActiveCallback(),
|
|
Client::SimpleFeedbackCallback());
|
|
|
|
std::cout << "Request ID: " << goal.id << std::endl;
|
|
LOG(INFO) << "Request ID: " << goal.id;
|
|
|
|
//wait for the action to return
|
|
bool finished_before_timeout = ac_.waitForResult(ros::Duration(30.0));
|
|
|
|
if (finished_before_timeout) {
|
|
actionlib::SimpleClientGoalState state = ac_.getState();
|
|
ROS_INFO("Action finished: %s", state.toString().c_str());
|
|
} else {
|
|
ROS_INFO("Action did not finish before the time out.");
|
|
}
|
|
}
|
|
|
|
private:
|
|
Client ac_;
|
|
long int factory_id_;
|
|
};
|
|
}
|