00001 #include "statepublisher_rostopic.h"
00002 #include "PlotJuggler/any.hpp"
00003 #include "../qnodedialog.h"
00004 #include "ros_type_introspection/ros_introspection.hpp"
00005 #include <QDialog>
00006 #include <QFormLayout>
00007 #include <QCheckBox>
00008 #include <QLabel>
00009 #include <QHBoxLayout>
00010 #include <QVBoxLayout>
00011 #include <QDialogButtonBox>
00012 #include <QScrollArea>
00013 #include <QPushButton>
00014 #include <QSettings>
00015 #include <QRadioButton>
00016 #include <rosbag/bag.h>
00017 #include <std_msgs/Header.h>
00018 #include <unordered_map>
00019 #include <rosgraph_msgs/Clock.h>
00020 #include <QMessageBox>
00021
00022 TopicPublisherROS::TopicPublisherROS():
00023 _enabled(false ),
00024 _node(nullptr),
00025 _publish_clock(true)
00026 {
00027 QSettings settings;
00028 _publish_clock = settings.value( "TopicPublisherROS/publish_clock", true ).toBool();
00029
00030 }
00031
00032 TopicPublisherROS::~TopicPublisherROS()
00033 {
00034 _enabled = false;
00035 }
00036
00037 void TopicPublisherROS::setParentMenu(QMenu *menu, QAction* action)
00038 {
00039 StatePublisher::setParentMenu( menu, action );
00040
00041 _enable_self_action = menu->actions().back();
00042
00043 _select_topics_to_publish = new QAction(QString("Select topics to be published"), _menu);
00044 _menu->addAction( _select_topics_to_publish );
00045 connect(_select_topics_to_publish, &QAction::triggered,
00046 this, &TopicPublisherROS::filterDialog);
00047 }
00048
00049 void TopicPublisherROS::setEnabled(bool to_enable)
00050 {
00051 if( !_node && to_enable)
00052 {
00053 _node = RosManager::getNode();
00054 }
00055 _enabled = (to_enable && _node);
00056
00057 if(_enabled)
00058 {
00059 filterDialog(true);
00060 if( !_tf_publisher )
00061 {
00062 _tf_publisher = std::unique_ptr<tf::TransformBroadcaster>( new tf::TransformBroadcaster );
00063 }
00064 _previous_play_index = std::numeric_limits<int>::max();
00065
00066 if( _publish_clock )
00067 {
00068 _clock_publisher = _node->advertise<rosgraph_msgs::Clock>( "/clock", 10, true);
00069 }
00070 else{
00071 _clock_publisher.shutdown();
00072 }
00073 }
00074 else{
00075 _node.reset();
00076 _publishers.clear();
00077 _clock_publisher.shutdown();
00078 }
00079
00080
00081 StatePublisher::setEnabled(_enabled);
00082 }
00083
00084 void TopicPublisherROS::filterDialog(bool autoconfirm)
00085 {
00086 auto all_topics = RosIntrospectionFactory::get().getTopicList();
00087
00088 if( all_topics.empty() ) return;
00089
00090 QDialog* dialog = new QDialog();
00091 dialog->setWindowTitle("Select topics to be published");
00092 dialog->setMinimumWidth(350);
00093 QVBoxLayout* vertical_layout = new QVBoxLayout();
00094 QFormLayout* grid_layout = new QFormLayout();
00095
00096 std::map<std::string, QCheckBox*> checkbox;
00097
00098 QFrame* frame = new QFrame;
00099
00100 auto publish_sim_time = new QRadioButton("Keep original timestamp and publish [/clock]");
00101 auto publish_real_time = new QRadioButton("Overwrite timestamp [std_msgs/Header/stamp]");
00102 QPushButton* select_button = new QPushButton("Select all");
00103 QPushButton* deselect_button = new QPushButton("Deselect all");
00104
00105 publish_sim_time->setChecked( _publish_clock );
00106 publish_sim_time->setFocusPolicy(Qt::NoFocus);
00107 publish_sim_time->setToolTip("Publish the topic [/clock].\n"
00108 "You might want to set rosparam use_sim_time = true" );
00109
00110 publish_real_time->setChecked( !_publish_clock );
00111 publish_real_time->setFocusPolicy(Qt::NoFocus);
00112 publish_real_time->setToolTip("Pretend it is a new message.\n"
00113 "The timestamp of the original message will be overwritten"
00114 "with ros::Time::Now()");
00115
00116 select_button->setFocusPolicy(Qt::NoFocus);
00117 deselect_button->setFocusPolicy(Qt::NoFocus);
00118
00119 for (const auto& topic: all_topics)
00120 {
00121 auto cb = new QCheckBox(dialog);
00122 auto filter_it = _topics_to_publish.find( *topic );
00123 if( filter_it == _topics_to_publish.end() )
00124 {
00125 cb->setChecked( true );
00126 }
00127 else{
00128 cb->setChecked( filter_it->second );
00129 }
00130 cb->setFocusPolicy(Qt::NoFocus);
00131 grid_layout->addRow( new QLabel( QString::fromStdString(*topic)), cb);
00132 checkbox.insert( std::make_pair(*topic, cb) );
00133 connect( select_button, &QPushButton::pressed, [cb](){ cb->setChecked(true);} );
00134 connect( deselect_button, &QPushButton::pressed, [cb](){ cb->setChecked(false);} );
00135 }
00136
00137 frame->setLayout(grid_layout);
00138
00139 QScrollArea* scrollArea = new QScrollArea;
00140 scrollArea->setWidget(frame);
00141
00142 QDialogButtonBox* buttons = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel);
00143
00144 QHBoxLayout* select_buttons_layout = new QHBoxLayout;
00145 select_buttons_layout->addWidget( select_button );
00146 select_buttons_layout->addWidget( deselect_button );
00147
00148 vertical_layout->addWidget( publish_sim_time );
00149 vertical_layout->addWidget( publish_real_time );
00150 vertical_layout->addWidget(scrollArea);
00151 vertical_layout->addLayout(select_buttons_layout);
00152 vertical_layout->addWidget( buttons );
00153
00154 connect(buttons, SIGNAL(accepted()), dialog, SLOT(accept()));
00155 connect(buttons, SIGNAL(rejected()), dialog, SLOT(reject()));
00156
00157 dialog->setLayout(vertical_layout);
00158
00159 if( !autoconfirm )
00160 {
00161 auto result = dialog->exec();
00162 }
00163
00164 if(autoconfirm || dialog->result() == QDialog::Accepted)
00165 {
00166 _topics_to_publish.clear();
00167 for(const auto& it: checkbox )
00168 {
00169 _topics_to_publish.insert( {it.first, it.second->isChecked() } );
00170 }
00171
00172
00173 for (auto it = _publishers.begin(); it != _publishers.end(); )
00174 {
00175 const std::string& topic_name = it->first;
00176 if( !toPublish(topic_name) )
00177 {
00178 it = _publishers.erase(it);
00179 }
00180 else{
00181 it++;
00182 }
00183 }
00184
00185 _publish_clock = publish_sim_time->isChecked();
00186
00187 QSettings settings;
00188 settings.setValue( "TopicPublisherROS/publish_clock", _publish_clock );
00189 }
00190 }
00191
00192 void TopicPublisherROS::broadcastTF(double current_time)
00193 {
00194 std::unordered_map<std::string, geometry_msgs::TransformStamped> transforms;
00195
00196 for(const auto& data_it: _datamap->user_defined )
00197 {
00198 const std::string& topic_name = data_it.first;
00199 const PlotDataAny& plot_any = data_it.second;
00200
00201 if( !toPublish(topic_name) )
00202 {
00203 continue;
00204 }
00205 const RosIntrospection::ShapeShifter* shapeshifter =
00206 RosIntrospectionFactory::get().getShapeShifter( topic_name );
00207 if( shapeshifter->getDataType() != "tf/tfMessage" &&
00208 shapeshifter->getDataType() != "tf2_msgs/TFMessage" )
00209 {
00210 continue;
00211 }
00212
00213 const PlotDataAny* tf_data = &plot_any;
00214 int last_index = tf_data->getIndexFromX( current_time );
00215 if( last_index < 0)
00216 {
00217 continue;
00218 }
00219
00220 std::vector<uint8_t> raw_buffer;
00221
00222 int initial_index = tf_data->getIndexFromX( current_time - 2.0 );
00223
00224 if( _previous_play_index < last_index &&
00225 _previous_play_index > initial_index )
00226 {
00227 initial_index = _previous_play_index;
00228 }
00229
00230 for(size_t index = std::max(0, initial_index); index <= last_index; index++ )
00231 {
00232 const nonstd::any& any_value = tf_data->at(index).y;
00233
00234 const bool isRosbagMessage = any_value.type() == typeid(rosbag::MessageInstance);
00235
00236 if( isRosbagMessage )
00237 {
00238 const auto& msg_instance = nonstd::any_cast<rosbag::MessageInstance>( any_value );
00239
00240 raw_buffer.resize( msg_instance.size() );
00241 ros::serialization::OStream ostream(raw_buffer.data(), raw_buffer.size());
00242 msg_instance.write(ostream);
00243
00244 tf::tfMessage tf_msg;
00245 ros::serialization::IStream istream( raw_buffer.data(), raw_buffer.size() );
00246 ros::serialization::deserialize(istream, tf_msg);
00247
00248 for(const auto& stamped_transform: tf_msg.transforms)
00249 {
00250 const auto& child_id = stamped_transform.child_frame_id;
00251 auto it = transforms.find(child_id);
00252 if( it == transforms.end())
00253 {
00254 transforms.insert( {stamped_transform.child_frame_id, stamped_transform} );
00255 }
00256 else if( it->second.header.stamp <= stamped_transform.header.stamp)
00257 {
00258 it->second = stamped_transform;
00259 }
00260 }
00261 }
00262 }
00263 }
00264
00265 std::vector<geometry_msgs::TransformStamped> transforms_vector;
00266 transforms_vector.reserve(transforms.size());
00267
00268 const auto now = ros::Time::now();
00269 for(auto& trans: transforms)
00270 {
00271 if( !_publish_clock )
00272 {
00273 trans.second.header.stamp = now;
00274 }
00275 transforms_vector.emplace_back( std::move(trans.second) );
00276 }
00277
00278 _tf_publisher->sendTransform(transforms_vector);
00279 }
00280
00281 bool TopicPublisherROS::toPublish(const std::string &topic_name)
00282 {
00283 auto it = _topics_to_publish.find( topic_name );
00284 if( it == _topics_to_publish.end() ){
00285
00286 return false;
00287 }
00288 else {
00289 return it->second;
00290 }
00291 }
00292
00293
00294 void TopicPublisherROS::publishAnyMsg(const rosbag::MessageInstance& msg_instance)
00295 {
00296 using namespace RosIntrospection;
00297
00298 const auto& topic_name = msg_instance.getTopic();
00299 RosIntrospection::ShapeShifter* shapeshifted =
00300 RosIntrospectionFactory::get().getShapeShifter( topic_name );
00301
00302 if( ! shapeshifted )
00303 {
00304 return;
00305 }
00306
00307 std::vector<uint8_t> raw_buffer;
00308 raw_buffer.resize( msg_instance.size() );
00309 ros::serialization::OStream ostream(raw_buffer.data(), raw_buffer.size());
00310 msg_instance.write(ostream);
00311
00312 if( !_publish_clock )
00313 {
00314 const ROSMessageInfo* msg_info = RosIntrospectionFactory::parser().getMessageInfo( topic_name );
00315 if(msg_info && msg_info->message_tree.croot()->children().size() >= 1)
00316 {
00317 const auto& first_field = msg_info->message_tree.croot()->child(0)->value();
00318 if(first_field->type().baseName() == "std_msgs/Header")
00319 {
00320 absl::Span<uint8_t> buffer(raw_buffer);
00321 std_msgs::Header msg;
00322 ros::serialization::IStream is( buffer.data(), buffer.size() );
00323 ros::serialization::deserialize(is, msg);
00324 msg.stamp = ros::Time::now();
00325 ros::serialization::OStream os( buffer.data(), buffer.size() );
00326 ros::serialization::serialize(os, msg);
00327 }
00328 }
00329 }
00330
00331 ros::serialization::IStream istream( raw_buffer.data(), raw_buffer.size() );
00332 shapeshifted->read( istream );
00333
00334 auto publisher_it = _publishers.find( topic_name );
00335 if( publisher_it == _publishers.end())
00336 {
00337 auto res = _publishers.insert( {topic_name, shapeshifted->advertise( *_node, topic_name, 10, true)} );
00338 publisher_it = res.first;
00339 }
00340
00341
00342 const ros::Publisher& publisher = publisher_it->second;
00343 publisher.publish( *shapeshifted );
00344 }
00345
00346
00347
00348 void TopicPublisherROS::updateState(double current_time)
00349 {
00350 if(!_enabled || !_node) return;
00351
00352 if( !ros::master::check() )
00353 {
00354 QMessageBox::warning(nullptr, tr("Disconnected!"),
00355 "The roscore master cannot be detected.\n"
00356 "The publisher will be disabled.");
00357 _enable_self_action->setChecked(false);
00358 return;
00359 }
00360
00361
00362 broadcastTF(current_time);
00363
00364
00365 auto data_it = _datamap->user_defined.find( "__consecutive_message_instances__" );
00366 if( data_it != _datamap->user_defined.end() )
00367 {
00368 const PlotDataAny& continuous_msgs = data_it->second;
00369 _previous_play_index = continuous_msgs.getIndexFromX(current_time);
00370
00371 }
00372
00373
00374 for(const auto& data_it: _datamap->user_defined )
00375 {
00376 const std::string& topic_name = data_it.first;
00377 const PlotDataAny& plot_any = data_it.second;
00378 if( !toPublish(topic_name) )
00379 {
00380 continue;
00381 }
00382
00383 const RosIntrospection::ShapeShifter* shapeshifter =
00384 RosIntrospectionFactory::get().getShapeShifter( topic_name );
00385
00386 if( shapeshifter->getDataType() == "tf/tfMessage" ||
00387 shapeshifter->getDataType() == "tf2_msgs/TFMessage" )
00388 {
00389 continue;
00390 }
00391
00392 int last_index = plot_any.getIndexFromX( current_time );
00393 if( last_index < 0)
00394 {
00395 continue;
00396 }
00397
00398 const auto& any_value = plot_any.at(last_index).y;
00399
00400 if( any_value.type() == typeid(rosbag::MessageInstance) )
00401 {
00402 const auto& msg_instance = nonstd::any_cast<rosbag::MessageInstance>( any_value );
00403 publishAnyMsg( msg_instance );
00404 }
00405 }
00406
00407 if( _publish_clock )
00408 {
00409 rosgraph_msgs::Clock clock;
00410 try{
00411 clock.clock.fromSec( current_time );
00412 _clock_publisher.publish( clock );
00413 }
00414 catch(...)
00415 {
00416 qDebug() << "error: " << current_time;
00417 }
00418 }
00419 }
00420
00421
00422 void TopicPublisherROS::play(double current_time)
00423 {
00424 if(!_enabled || !_node) return;
00425
00426 if( !ros::master::check() )
00427 {
00428 QMessageBox::warning(nullptr, tr("Disconnected!"),
00429 "The roscore master cannot be detected.\n"
00430 "The publisher will be disabled.");
00431 _enable_self_action->setChecked(false);
00432 return;
00433 }
00434
00435 auto data_it = _datamap->user_defined.find( "__consecutive_message_instances__" );
00436 if( data_it == _datamap->user_defined.end() )
00437 {
00438 return;
00439 }
00440 const PlotDataAny& continuous_msgs = data_it->second;
00441 int current_index = continuous_msgs.getIndexFromX(current_time);
00442
00443 if( _previous_play_index > current_index)
00444 {
00445 _previous_play_index = current_index;
00446 updateState(current_time);
00447 return;
00448 }
00449 else
00450 {
00451 const PlotDataAny& consecutive_msg = data_it->second;
00452 for(int index = _previous_play_index+1; index <= current_index; index++)
00453 {
00454
00455 const auto& any_value = consecutive_msg.at(index).y;
00456 if( any_value.type() == typeid(rosbag::MessageInstance) )
00457 {
00458 const auto& msg_instance = nonstd::any_cast<rosbag::MessageInstance>( any_value );
00459
00460 if( !toPublish( msg_instance.getTopic() ) )
00461 {
00462 continue;
00463 }
00464
00465
00466 publishAnyMsg( msg_instance );
00467
00468 if( _publish_clock )
00469 {
00470 rosgraph_msgs::Clock clock;
00471 clock.clock = msg_instance.getTime();
00472 _clock_publisher.publish( clock );
00473 }
00474 }
00475 }
00476 _previous_play_index = current_index;
00477 }
00478 }