-
Notifications
You must be signed in to change notification settings - Fork 45
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'development' of https://github.com/PlotJuggler/plotjugg…
…ler-ros-plugins into development
- Loading branch information
Showing
14 changed files
with
196 additions
and
40 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,7 +1,7 @@ | ||
| <?xml version="1.0"?> | ||
| <package format="3"> | ||
| <name>plotjuggler_ros</name> | ||
| <version>1.0.0</version> | ||
| <version>1.0.1</version> | ||
| <description>PlotJuggler plugin for ROS</description> | ||
|
|
||
| <maintainer email="[email protected]">Davide Faconti</maintainer> | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,54 @@ | ||
| #pragma once | ||
|
|
||
| #include <diagnostic_msgs/msg/diagnostic_array.hpp> | ||
| #include <boost/spirit/include/qi.hpp> | ||
| #include "ros2_parser.h" | ||
| #include "fmt/format.h" | ||
|
|
||
| class DiagnosticMsgParser : public BuiltinMessageParser<diagnostic_msgs::msg::DiagnosticArray> | ||
| { | ||
| public: | ||
| DiagnosticMsgParser(const std::string& topic_name, PJ::PlotDataMapRef& plot_data) | ||
| : BuiltinMessageParser<diagnostic_msgs::msg::DiagnosticArray>(topic_name, plot_data) | ||
| { | ||
| } | ||
|
|
||
| virtual void parseMessageImpl(const diagnostic_msgs::msg::DiagnosticArray& msg, double timestamp) override | ||
| { | ||
| double header_stamp = double(msg.header.stamp.sec) + double(msg.header.stamp.nanosec) * 1e-9; | ||
| timestamp = (_use_header_stamp && header_stamp > 0) ? header_stamp : timestamp; | ||
|
|
||
| auto stamp_series = &getSeries("/header/seq"); | ||
| stamp_series->pushBack( {timestamp, header_stamp} ); | ||
|
|
||
| std::string key; | ||
|
|
||
| for (const auto& status : msg.status) | ||
| { | ||
| for (const auto& kv : status.values) | ||
| { | ||
| const char* start_ptr = kv.value.data(); | ||
| double val = 0; | ||
|
|
||
| bool parsed = boost::spirit::qi::parse(start_ptr, start_ptr + kv.value.size(), | ||
| boost::spirit::qi::double_, val); | ||
| if (!parsed){ | ||
| continue; | ||
| } | ||
|
|
||
| if (status.hardware_id.empty()) | ||
| { | ||
| key = fmt::format("{}/{}/{}", _topic_name, status.name, kv.key); | ||
| } | ||
| else | ||
| { | ||
| key = fmt::format("{}/{}/{}/{}", _topic_name, status.hardware_id, status.name, kv.key); | ||
| } | ||
| auto& series = getSeries(key); | ||
| series.pushBack({ timestamp, val }); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| }; | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,63 @@ | ||
| #pragma once | ||
|
|
||
| #include <tf2_msgs/msg/tf_message.hpp> | ||
| #include "fmt/format.h" | ||
| #include "ros2_parser.h" | ||
|
|
||
| class TfMsgParser : public BuiltinMessageParser<tf2_msgs::msg::TFMessage> | ||
| { | ||
| public: | ||
|
|
||
| TfMsgParser(const std::string& topic_name, PJ::PlotDataMapRef& plot_data) : | ||
| BuiltinMessageParser<tf2_msgs::msg::TFMessage>(topic_name, plot_data) | ||
| { | ||
| } | ||
|
|
||
| void parseMessageImpl(const tf2_msgs::msg::TFMessage& msg, double timestamp) override | ||
| { | ||
| using namespace PJ; | ||
|
|
||
| for (const auto& trans : msg.transforms) | ||
| { | ||
| double header_stamp = double(trans.header.stamp.sec) + double(trans.header.stamp.nanosec) * 1e-9; | ||
| timestamp = (_use_header_stamp && header_stamp > 0) ? header_stamp : timestamp; | ||
|
|
||
| std::string prefix; | ||
| if (trans.header.frame_id.empty()) | ||
| { | ||
| prefix = fmt::format("{}/{}", _topic_name, trans.child_frame_id); | ||
| } | ||
| else | ||
| { | ||
| prefix = fmt::format("{}/{}/{}", _topic_name, trans.header.frame_id, trans.child_frame_id); | ||
| } | ||
|
|
||
| PlotData* series = &getSeries( prefix + "/header/stamp"); | ||
| series->pushBack({ timestamp, header_stamp }); | ||
|
|
||
| series = &getSeries( prefix + "/translation/x"); | ||
| series->pushBack({ timestamp, trans.transform.translation.x }); | ||
|
|
||
| series = &getSeries( prefix + "/translation/y"); | ||
| series->pushBack({ timestamp, trans.transform.translation.y }); | ||
|
|
||
| series = &getSeries( prefix + "/translation/z"); | ||
| series->pushBack({ timestamp, trans.transform.translation.z }); | ||
|
|
||
| series = &getSeries( prefix + "/rotation/x"); | ||
| series->pushBack({ timestamp, trans.transform.rotation.x }); | ||
|
|
||
| series = &getSeries( prefix + "/rotation/y"); | ||
| series->pushBack({ timestamp, trans.transform.rotation.y }); | ||
|
|
||
| series = &getSeries( prefix + "/rotation/z"); | ||
| series->pushBack({ timestamp, trans.transform.rotation.z }); | ||
|
|
||
| series = &getSeries( prefix + "/rotation/w"); | ||
| series->pushBack({ timestamp, trans.transform.rotation.w }); | ||
| } | ||
| } | ||
| private: | ||
|
|
||
| }; | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters