Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 26 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ elseif(ament_cmake_FOUND)
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rclcpp REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
msg/DataPoint.msg
Expand All @@ -46,6 +47,31 @@ elseif(ament_cmake_FOUND)
ADD_LINTER_TESTS
)


add_executable(example example.cpp)

target_include_directories(example PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)

rosidl_get_typesupport_target(cpp_typesupport_target
${PROJECT_NAME} "rosidl_typesupport_cpp")
target_link_libraries(example "${cpp_typesupport_target}")

ament_target_dependencies(example rclcpp std_msgs)


install(DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
)

ament_export_include_directories(
include
)
install(TARGETS
example
DESTINATION lib/${PROJECT_NAME})

ament_export_dependencies(rosidl_default_runtime)
ament_export_dependencies(std_msgs)
ament_package()
Expand Down
41 changes: 15 additions & 26 deletions example.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,54 +3,43 @@
#include <rclcpp/rclcpp.hpp>
#include <random>
#include <cmath>
#include "pj_msgs/msg/dictionary.hpp"
#include "pj_msgs/msg/data_points.hpp"
#include "plotjuggler_msgs/dictionary.hpp"
#include "plotjuggler_msgs/data_point.hpp"
#include "plotjuggler_msgs/data_points.hpp"

using namespace std::chrono_literals;

pj_msgs::msg::DataPoint CreateDataPoint(uint16_t name_index, double time, double value)
{
pj_msgs::msg::DataPoint point;
point.stamp = time;
point.name_index = name_index;
point.value = value;
return point;
}

int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("pj_test");

auto publisher_dict = node->create_publisher<pj_msgs::msg::Dictionary>("pj_msg_dictionary",
auto publisher_dict = node->create_publisher<plotjuggler_msgs::msg::Dictionary>("pj_msg_dictionary",
rclcpp::QoS(10).transient_local());
auto publisher = node->create_publisher<pj_msgs::msg::DataPoints>("pj_msg_data", 10);
auto publisher = node->create_publisher<plotjuggler_msgs::msg::DataPoints>("pj_msg_data", 10);

std::random_device rd;
const uint32_t UUID = rd(); // just a random number

pj_msgs::msg::Dictionary dictionary;

dictionary.dictionary_uuid = UUID;
dictionary.names.push_back("sensor_a"); // index 0
dictionary.names.push_back("sensor_b"); // index 1
dictionary.names.push_back("sensor_c"); // index 2
plotjuggler_msgs::Dictionary dictionary(UUID);
dictionary.push_back("sensor_a"); // index 0
dictionary.push_back("sensor_b"); // index 1
dictionary.push_back("sensor_c"); // index 2

publisher_dict->publish(dictionary);
publisher_dict->publish(dictionary.msg());


rclcpp::WallRate loop_rate(50ms);
double t=0;
while (rclcpp::ok()) {

t += 0.1;
pj_msgs::msg::DataPoints msg;
msg.dictionary_uuid = UUID;
msg.samples.push_back( CreateDataPoint(0, t, std::sin(t)));
msg.samples.push_back( CreateDataPoint(1, t, std::cos(t)));
msg.samples.push_back( CreateDataPoint(2, t, 2*std::cos(t)));
plotjuggler_msgs::DataPoints points(UUID);
points.samples.push_back( plotjuggler_msgs::DataPoint(0, t, std::sin(t)).msg());
points.samples.push_back( plotjuggler_msgs::DataPoint(1, t, std::cos(t)).msg());
points.samples.push_back( plotjuggler_msgs::DataPoint(2, t, 2*std::cos(t)).msg());

publisher->publish(msg);
publisher->publish(points.msg());
rclcpp::spin_some(node);
loop_rate.sleep();
}
Expand Down
25 changes: 25 additions & 0 deletions include/plotjuggler_msgs/data_point.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
#ifndef PLOTJUGGLER_MSGS__DATA_POINT_HPP_
#define PLOTJUGGLER_MSGS__DATA_POINT_HPP_

#include <plotjuggler_msgs/msg/data_point.hpp>

namespace plotjuggler_msgs
{
struct DataPoint : public plotjuggler_msgs::msg::DataPoint
{
DataPoint();
DataPoint(uint16_t name_index, double time, double value)
{
this->stamp = time;
this->name_index = name_index;
this->value = value;
}
const plotjuggler_msgs::msg::DataPoint& msg() const {
return static_cast<const plotjuggler_msgs::msg::DataPoint&>(*this);
}
plotjuggler_msgs::msg::DataPoint& msg() {
return static_cast<plotjuggler_msgs::msg::DataPoint&>(*this);
}
};
} // namespace plotjuggler_msgs
#endif // PLOTJUGGLER_MSGS__DATA_POINT_HPP_
26 changes: 26 additions & 0 deletions include/plotjuggler_msgs/data_points.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
#ifndef PLOTJUGGLER_MSGS__DATA_POINTS_HPP_
#define PLOTJUGGLER_MSGS__DATA_POINTS_HPP_

#include <plotjuggler_msgs/msg/data_points.hpp>

namespace plotjuggler_msgs
{
struct DataPoints : public plotjuggler_msgs::msg::DataPoints
{
DataPoints();
DataPoints(uint32_t dictionary_uuid){
this->dictionary_uuid = dictionary_uuid;
}

void push_back(const plotjuggler_msgs::msg::DataPoint& data_point){
this->samples.push_back(data_point);
}
const plotjuggler_msgs::msg::DataPoints& msg() const {
return static_cast<const plotjuggler_msgs::msg::DataPoints&>(*this);
}
plotjuggler_msgs::msg::DataPoints& msg() {
return static_cast< plotjuggler_msgs::msg::DataPoints&>(*this);
}
};
} // namespace plotjuggler_msgs
#endif // PLOTJUGGLER_MSGS__DATA_POINTS_HPP_
25 changes: 25 additions & 0 deletions include/plotjuggler_msgs/dictionary.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
#ifndef PLOTJUGGLER_MSGS__DICTIONARY_HPP_
#define PLOTJUGGLER_MSGS__DICTIONARY_HPP_

#include <plotjuggler_msgs/msg/dictionary.hpp>

namespace plotjuggler_msgs
{
struct Dictionary : public plotjuggler_msgs::msg::Dictionary
{
Dictionary();
Dictionary(uint32_t dictionary_uuid){
this->dictionary_uuid = dictionary_uuid;
}
void push_back(const std::string& name){
this->names.push_back(name);
}
const plotjuggler_msgs::msg::Dictionary& msg() const {
return static_cast<const plotjuggler_msgs::msg::Dictionary&>(*this);
}
plotjuggler_msgs::msg::Dictionary& msg() {
return static_cast<plotjuggler_msgs::msg::Dictionary&>(*this);
}
};
} // namespace plotjuggler_msgs
#endif // PLOTJUGGLER_MSGS__DICTIONARY_HPP_
1 change: 1 addition & 0 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">rosidl_default_generators</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">rclcpp</buildtool_depend>

<depend condition="$ROS_VERSION == 1">message_generation</depend>
<depend condition="$ROS_VERSION == 1">message_runtime</depend>
Expand Down