Commit ebc51723 authored by James Ward's avatar James Ward
Browse files

Use ActionLib for optimisation

Stops RViz panel from blocking when optimising.
parent 089053ea
......@@ -5,6 +5,8 @@ set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
find_package(catkin REQUIRED COMPONENTS
actionlib
actionlib_msgs
cv_bridge
dynamic_reconfigure
eigen_conversions
......@@ -42,12 +44,18 @@ set(CMAKE_AUTOMOC ON)
# FILES
# extrinsics.msg
# )
add_action_files(
FILES
RunOptimise.action
)
add_service_files(
FILES
Optimise.srv
)
generate_messages(
DEPENDENCIES
actionlib_msgs
geometry_msgs
std_msgs
sensor_msgs
)
......
# Goal
---
# Result
geometry_msgs/Transform transform
---
# Feedback
......@@ -14,6 +14,8 @@
#include <pcl/point_cloud.h>
#include <pcl/ModelCoefficients.h>
#include <actionlib/server/simple_action_server.h>
#include <cam_lidar_calibration/RunOptimiseAction.h>
#include <cam_lidar_calibration/Optimise.h>
#include <cam_lidar_calibration/boundsConfig.h>
......@@ -40,6 +42,9 @@ public:
bool serviceCB(Optimise::Request& req, Optimise::Response& res);
void boundsCB(boundsConfig& config, uint32_t level);
void optimise(const RunOptimiseGoalConstPtr& goal,
actionlib::SimpleActionServer<cam_lidar_calibration::RunOptimiseAction>* as);
void visualiseSamples();
private:
......
......@@ -8,6 +8,8 @@
<buildtool_depend>catkin</buildtool_depend>
<depend>actionlib</depend>
<depend>actionlib_msgs</depend>
<depend>cv_bridge</depend>
<depend>eigen_conversions</depend>
<depend>image_transport</depend>
......
......@@ -2,15 +2,17 @@
#include <QHBoxLayout>
#include <QVBoxLayout>
#include <QGroupBox>
#include <QTimer>
#include "cam_lidar_panel.h"
#include <cam_lidar_calibration/Optimise.h>
namespace cam_lidar_calibration
{
CamLidarPanel::CamLidarPanel(QWidget* parent) : rviz::Panel(parent)
CamLidarPanel::CamLidarPanel(QWidget* parent) : rviz::Panel(parent), action_client_("run_optimise", true)
{
optimise_client_ = nh_.serviceClient<cam_lidar_calibration::Optimise>("optimiser");
action_client_.waitForServer();
QVBoxLayout* main_layout = new QVBoxLayout;
QHBoxLayout* button_layout = new QHBoxLayout;
......@@ -21,6 +23,9 @@ CamLidarPanel::CamLidarPanel(QWidget* parent) : rviz::Panel(parent)
optimise_button_ = new QPushButton("Optimise");
optimise_button_->setEnabled(false);
connect(optimise_button_, SIGNAL(clicked()), this, SLOT(optimise()));
QTimer* timer = new QTimer;
connect(timer, SIGNAL(timeout()), this, SLOT(updateResult()));
timer->start(500);
output_label_ = new QLabel("");
......@@ -67,16 +72,36 @@ void CamLidarPanel::discardSample()
void CamLidarPanel::optimise()
{
Optimise srv;
srv.request.operation = Optimise::Request::OPTIMISE;
optimise_client_.call(srv);
auto t = srv.response.transform.translation;
auto r = srv.response.transform.rotation;
std::ostringstream os;
os.precision(3);
os << "Rotation - w: " << r.w << " x: " << r.x << " y: " << r.y << " z: " << r.z;
os << "\nTranslation - x: " << t.x << " y: " << t.y << " z: " << t.z;
output_label_->setText(QString::fromStdString(os.str()));
RunOptimiseGoal goal;
action_client_.sendGoal(goal);
}
void CamLidarPanel::updateResult()
{
if (action_client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
{
capture_button_->setEnabled(true);
discard_button_->setEnabled(true);
optimise_button_->setEnabled(true);
auto result = action_client_.getResult();
auto t = result->transform.translation;
auto r = result->transform.rotation;
std::ostringstream os;
os.precision(3);
os << "Rotation - w: " << r.w << " x: " << r.x << " y: " << r.y << " z: " << r.z;
os << "\nTranslation - x: " << t.x << " y: " << t.y << " z: " << t.z;
output_label_->setText(QString::fromStdString(os.str()));
}
if (action_client_.getState() == actionlib::SimpleClientGoalState::ACTIVE)
{
capture_button_->setEnabled(false);
discard_button_->setEnabled(false);
optimise_button_->setEnabled(false);
std::string str = "Optimising...";
// Make the ellipsis "bounce"
int count = (output_label_->text().length() + 2) % 3;
output_label_->setText(QString::fromStdString(str.substr(0, 11 + count)));
}
}
// Save all configuration data from this panel to the given
......
......@@ -10,6 +10,11 @@
#include <QLabel>
#include <QPushButton>
#include <actionlib/client/simple_action_client.h>
#include <cam_lidar_calibration/RunOptimiseAction.h>
using ActionClient = actionlib::SimpleActionClient<cam_lidar_calibration::RunOptimiseAction>;
namespace cam_lidar_calibration
{
class CamLidarPanel : public rviz::Panel
......@@ -38,11 +43,13 @@ protected Q_SLOTS:
void captureSample();
void discardSample();
void optimise();
void updateResult();
protected:
// The ROS node handle.
ros::NodeHandle nh_;
ros::ServiceClient optimise_client_;
ActionClient action_client_;
QLabel* output_label_;
QPushButton* capture_button_;
......
#include "cam_lidar_calibration/feature_extractor.h"
#include <actionlib/server/simple_action_server.h>
#include <cam_lidar_calibration/RunOptimiseAction.h>
using actionlib::SimpleActionServer;
using cam_lidar_calibration::FeatureExtractor;
int main(int argc, char** argv)
{
// Initialize Node and handles
ros::init(argc, argv, "FeatureExtractor");
ros::NodeHandle n;
cam_lidar_calibration::FeatureExtractor feature_extractor;
FeatureExtractor feature_extractor;
SimpleActionServer<cam_lidar_calibration::RunOptimiseAction> optimise_action(
n, "run_optimise", boost::bind(&FeatureExtractor::optimise, feature_extractor, _1, &optimise_action), false);
optimise_action.start();
ros::Rate loop_rate(10);
ros::AsyncSpinner spinner(4);
......
......@@ -84,20 +84,6 @@ bool FeatureExtractor::serviceCB(Optimise::Request& req, Optimise::Response& res
pc_samples_.pop_back();
}
break;
case Optimise::Request::OPTIMISE:
if (!optimiser_->samples_.empty())
{
ROS_INFO("Calling optimiser");
auto t = optimiser_->optimise();
res.transform.translation.x = t.x / 1000.;
res.transform.translation.y = t.y / 1000.;
res.transform.translation.z = t.z / 1000.;
Eigen::Matrix3d mat;
cv::cv2eigen(t.rot.toMat(), mat);
Eigen::Quaterniond quat(mat);
tf::quaternionEigenToMsg(quat, res.transform.rotation);
}
break;
}
publishBoardPointCloud();
flag = req.operation; // read flag published by rviz calibration panel
......@@ -109,6 +95,21 @@ bool FeatureExtractor::serviceCB(Optimise::Request& req, Optimise::Response& res
return true;
}
void FeatureExtractor::optimise(const RunOptimiseGoalConstPtr& goal,
actionlib::SimpleActionServer<RunOptimiseAction>* as)
{
auto t = optimiser_->optimise();
RunOptimiseResult res;
res.transform.translation.x = t.x / 1000.;
res.transform.translation.y = t.y / 1000.;
res.transform.translation.z = t.z / 1000.;
Eigen::Matrix3d mat;
cv::cv2eigen(t.rot.toMat(), mat);
Eigen::Quaterniond quat(mat);
tf::quaternionEigenToMsg(quat, res.transform.rotation);
as->setSucceeded(res);
}
void FeatureExtractor::publishBoardPointCloud()
{
// Publish collected board clouds
......
......@@ -2,9 +2,6 @@ int8 operation
int8 READY=0
int8 CAPTURE=1
int8 OPTIMISE=2
int8 DISCARD=3
int8 USE=4
int8 DISCARD=2
---
uint8 samples
geometry_msgs/Transform transform
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