Skip to content

Commit

Permalink
Merge pull request #179 from ros2/uncrustify_master
Browse files Browse the repository at this point in the history
update style to match latest uncrustify
  • Loading branch information
dirk-thomas authored Sep 29, 2017
2 parents 4444f34 + bc97923 commit 5ba0e66
Show file tree
Hide file tree
Showing 12 changed files with 193 additions and 171 deletions.
122 changes: 62 additions & 60 deletions composition/src/api_composition.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -82,76 +82,78 @@ int main(int argc, char * argv[])
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<composition::srv::LoadNode::Request> request,
std::shared_ptr<composition::srv::LoadNode::Response> response)
{
// get node plugin resource from package
std::string content;
std::string base_path;
if (!ament_index_cpp::get_resource("node_plugin", request->package_name, content, &base_path)) {
fprintf(stderr, "Could not find requested resource in ament index\n");
response->success = false;
return;
}

std::vector<std::string> lines = split(content, '\n', true);
for (auto line : lines) {
std::vector<std::string> parts = split(line, ';');
if (parts.size() != 2) {
fprintf(stderr, "Invalid resource entry\n");
{
// get node plugin resource from package
std::string content;
std::string base_path;
if (
!ament_index_cpp::get_resource("node_plugin", request->package_name, content, &base_path))
{
fprintf(stderr, "Could not find requested resource in ament index\n");
response->success = false;
return;
}
// match plugin name with the same rmw suffix as this executable
if (parts[0] != request->plugin_name) {
continue;
}

std::string class_name = parts[0];
std::vector<std::string> lines = split(content, '\n', true);
for (auto line : lines) {
std::vector<std::string> parts = split(line, ';');
if (parts.size() != 2) {
fprintf(stderr, "Invalid resource entry\n");
response->success = false;
return;
}
// match plugin name with the same rmw suffix as this executable
if (parts[0] != request->plugin_name) {
continue;
}

// load node plugin
std::string library_path = parts[1];
if (!fs::path(library_path).is_absolute()) {
library_path = base_path + "/" + library_path;
}
printf("Load library %s\n", library_path.c_str());
class_loader::ClassLoader * loader;
try {
loader = new class_loader::ClassLoader(library_path);
} catch (const std::exception & ex) {
fprintf(stderr, "Failed to load library: %s\n", ex.what());
response->success = false;
return;
} catch (...) {
fprintf(stderr, "Failed to load library\n");
response->success = false;
return;
}
auto classes = loader->getAvailableClasses<rclcpp::Node>();
for (auto clazz : classes) {
if (clazz == class_name) {
printf("Instantiate class %s\n", clazz.c_str());
auto node = loader->createInstance<rclcpp::Node>(clazz);
exec.add_node(node);
nodes.push_back(node);
loaders.push_back(loader);
response->success = true;
std::string class_name = parts[0];

// load node plugin
std::string library_path = parts[1];
if (!fs::path(library_path).is_absolute()) {
library_path = base_path + "/" + library_path;
}
printf("Load library %s\n", library_path.c_str());
class_loader::ClassLoader * loader;
try {
loader = new class_loader::ClassLoader(library_path);
} catch (const std::exception & ex) {
fprintf(stderr, "Failed to load library: %s\n", ex.what());
response->success = false;
return;
} catch (...) {
fprintf(stderr, "Failed to load library\n");
response->success = false;
return;
}
}
auto classes = loader->getAvailableClasses<rclcpp::Node>();
for (auto clazz : classes) {
if (clazz == class_name) {
printf("Instantiate class %s\n", clazz.c_str());
auto node = loader->createInstance<rclcpp::Node>(clazz);
exec.add_node(node);
nodes.push_back(node);
loaders.push_back(loader);
response->success = true;
return;
}
}

// no matching class found in loader
delete loader;
// no matching class found in loader
delete loader;
fprintf(
stderr, "Failed to find class with the requested plugin name '%s' in "
"the loaded library\n",
request->plugin_name.c_str());
response->success = false;
return;
}
fprintf(
stderr, "Failed to find class with the requested plugin name '%s' in "
"the loaded library\n",
request->plugin_name.c_str());
stderr, "Failed to find plugin name '%s' in prefix '%s'\n",
request->plugin_name.c_str(), base_path.c_str());
response->success = false;
return;
}
fprintf(
stderr, "Failed to find plugin name '%s' in prefix '%s'\n",
request->plugin_name.c_str(), base_path.c_str());
response->success = false;
});
});

exec.spin();

Expand Down
26 changes: 14 additions & 12 deletions demo_nodes_cpp/src/timers/one_off_timer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,18 +26,20 @@ class OneOffTimerNode : public rclcpp::Node
OneOffTimerNode()
: rclcpp::Node("one_off_timer"), count(0)
{
periodic_timer = this->create_wall_timer(2s, [this]() {
printf("in periodic_timer callback\n");
if (this->count++ % 3 == 0) {
printf(" resetting one off timer\n");
this->one_off_timer = this->create_wall_timer(1s, [this]() {
printf("in one_off_timer callback\n");
this->one_off_timer->cancel();
});
} else {
printf(" not resetting one off timer\n");
}
});
periodic_timer = this->create_wall_timer(
2s,
[this]() {
printf("in periodic_timer callback\n");
if (this->count++ % 3 == 0) {
printf(" resetting one off timer\n");
this->one_off_timer = this->create_wall_timer(1s, [this]() {
printf("in one_off_timer callback\n");
this->one_off_timer->cancel();
});
} else {
printf(" not resetting one off timer\n");
}
});
}

rclcpp::TimerBase::SharedPtr periodic_timer;
Expand Down
30 changes: 17 additions & 13 deletions demo_nodes_cpp/src/timers/reuse_timer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,22 +26,26 @@ class OneOffTimerNode : public rclcpp::Node
OneOffTimerNode()
: rclcpp::Node("reuse_timer"), count(0)
{
one_off_timer = this->create_wall_timer(1s, [this]() {
printf("in one_off_timer callback\n");
this->one_off_timer->cancel();
});
one_off_timer = this->create_wall_timer(
1s,
[this]() {
printf("in one_off_timer callback\n");
this->one_off_timer->cancel();
});
// cancel immediately to prevent it running the first time.
one_off_timer->cancel();

periodic_timer = this->create_wall_timer(2s, [this]() {
printf("in periodic_timer callback\n");
if (this->count++ % 3 == 0) {
printf(" resetting one off timer\n");
this->one_off_timer->reset();
} else {
printf(" not resetting one off timer\n");
}
});
periodic_timer = this->create_wall_timer(
2s,
[this]() {
printf("in periodic_timer callback\n");
if (this->count++ % 3 == 0) {
printf(" resetting one off timer\n");
this->one_off_timer->reset();
} else {
printf(" not resetting one off timer\n");
}
});
}

rclcpp::TimerBase::SharedPtr periodic_timer;
Expand Down
12 changes: 7 additions & 5 deletions demo_nodes_cpp/src/topics/allocator_tutorial.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -73,14 +73,16 @@ struct MyAllocator
};

template<typename T, typename U>
constexpr bool operator==(const MyAllocator<T> &,
constexpr bool operator==(
const MyAllocator<T> &,
const MyAllocator<U> &) noexcept
{
return true;
}

template<typename T, typename U>
constexpr bool operator!=(const MyAllocator<T> &,
constexpr bool operator!=(
const MyAllocator<T> &,
const MyAllocator<U> &) noexcept
{
return false;
Expand Down Expand Up @@ -163,9 +165,9 @@ int main(int argc, char ** argv)
// Create a custom allocator and pass the allocator to the publisher and subscriber.
auto alloc = std::make_shared<MyAllocator<void>>();
auto publisher = node->create_publisher<std_msgs::msg::UInt32>("allocator_tutorial", 10, alloc);
auto msg_mem_strat =
std::make_shared<rclcpp::message_memory_strategy::MessageMemoryStrategy<std_msgs::msg::UInt32,
MyAllocator<>>>(alloc);
auto msg_mem_strat = std::make_shared<
rclcpp::message_memory_strategy::MessageMemoryStrategy<
std_msgs::msg::UInt32, MyAllocator<>>>(alloc);
auto subscriber = node->create_subscription<std_msgs::msg::UInt32>(
"allocator_tutorial", 10, callback, nullptr, false, msg_mem_strat, alloc);

Expand Down
8 changes: 5 additions & 3 deletions demo_nodes_cpp/src/topics/listener_best_effort.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +23,11 @@ int main(int argc, char * argv[])
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("listener");
auto sub = node->create_subscription<std_msgs::msg::String>(
"chatter", [](const std_msgs::msg::String::SharedPtr msg) -> void {
printf("I heard: [%s]\n", msg->data.c_str());
}, rmw_qos_profile_sensor_data);
"chatter",
[](const std_msgs::msg::String::SharedPtr msg) -> void {
printf("I heard: [%s]\n", msg->data.c_str());
},
rmw_qos_profile_sensor_data);
rclcpp::spin(node);
return 0;
}
3 changes: 2 additions & 1 deletion intra_process_demo/include/image_pipeline/camera_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,8 @@
class CameraNode : public rclcpp::Node
{
public:
CameraNode(const std::string & output, const std::string & node_name = "camera_node",
CameraNode(
const std::string & output, const std::string & node_name = "camera_node",
bool watermark = true, int device = 0, int width = 320, int height = 240)
: Node(node_name, "", true), canceled_(false), watermark_(watermark)
{
Expand Down
52 changes: 27 additions & 25 deletions intra_process_demo/include/image_pipeline/image_view_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,32 +34,34 @@ class ImageViewNode : public rclcpp::Node
{
// Create a subscription on the input topic.
sub_ = this->create_subscription<sensor_msgs::msg::Image>(
input, [node_name, watermark](const sensor_msgs::msg::Image::SharedPtr msg) {
// Create a cv::Mat from the image message (without copying).
cv::Mat cv_mat(
msg->height, msg->width,
encoding2mat_type(msg->encoding),
msg->data.data());
if (watermark) {
// Annotate with the pid and pointer address.
std::stringstream ss;
ss << "pid: " << GETPID() << ", ptr: " << msg.get();
draw_on_image(cv_mat, ss.str(), 60);
}
// Show the image.
CvMat c_mat = cv_mat;
cvShowImage(node_name.c_str(), &c_mat);
char key = cv::waitKey(1); // Look for key presses.
if (key == 27 /* ESC */ || key == 'q') {
rclcpp::shutdown();
}
if (key == ' ') { // If <space> then pause until another <space>.
key = '\0';
while (key != ' ') {
key = cv::waitKey(1);
input,
[node_name, watermark](const sensor_msgs::msg::Image::SharedPtr msg) {
// Create a cv::Mat from the image message (without copying).
cv::Mat cv_mat(
msg->height, msg->width,
encoding2mat_type(msg->encoding),
msg->data.data());
if (watermark) {
// Annotate with the pid and pointer address.
std::stringstream ss;
ss << "pid: " << GETPID() << ", ptr: " << msg.get();
draw_on_image(cv_mat, ss.str(), 60);
}
}
}, rmw_qos_profile_sensor_data);
// Show the image.
CvMat c_mat = cv_mat;
cvShowImage(node_name.c_str(), &c_mat);
char key = cv::waitKey(1); // Look for key presses.
if (key == 27 /* ESC */ || key == 'q') {
rclcpp::shutdown();
}
if (key == ' ') { // If <space> then pause until another <space>.
key = '\0';
while (key != ' ') {
key = cv::waitKey(1);
}
}
},
rmw_qos_profile_sensor_data);
}

private:
Expand Down
34 changes: 18 additions & 16 deletions intra_process_demo/include/image_pipeline/watermark_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,22 +39,24 @@ class WatermarkNode : public rclcpp::Node
std::weak_ptr<std::remove_pointer<decltype(pub_.get())>::type> captured_pub = pub_;
// Create a subscription on the output topic.
sub_ = this->create_subscription<sensor_msgs::msg::Image>(
input, [captured_pub, text](sensor_msgs::msg::Image::UniquePtr msg) {
auto pub_ptr = captured_pub.lock();
if (!pub_ptr) {
return;
}
// Create a cv::Mat from the image message (without copying).
cv::Mat cv_mat(
msg->height, msg->width,
encoding2mat_type(msg->encoding),
msg->data.data());
// Annotate the image with the pid, pointer address, and the watermark text.
std::stringstream ss;
ss << "pid: " << GETPID() << ", ptr: " << msg.get() << " " << text;
draw_on_image(cv_mat, ss.str(), 40);
pub_ptr->publish(msg); // Publish it along.
}, qos);
input,
[captured_pub, text](sensor_msgs::msg::Image::UniquePtr msg) {
auto pub_ptr = captured_pub.lock();
if (!pub_ptr) {
return;
}
// Create a cv::Mat from the image message (without copying).
cv::Mat cv_mat(
msg->height, msg->width,
encoding2mat_type(msg->encoding),
msg->data.data());
// Annotate the image with the pid, pointer address, and the watermark text.
std::stringstream ss;
ss << "pid: " << GETPID() << ", ptr: " << msg.get() << " " << text;
draw_on_image(cv_mat, ss.str(), 40);
pub_ptr->publish(msg); // Publish it along.
},
qos);
}

private:
Expand Down
Loading

0 comments on commit 5ba0e66

Please sign in to comment.