Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Helper functions for geometry_msgs::Vector3 and std_msgs::ColorRGBA #18

Open
wants to merge 3 commits into
base: melodic-devel
Choose a base branch
from
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
3 changes: 3 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,9 @@ example:
param6: [2, 2, 2, 1, 0, 0, 0] # Eigen::Isometry3d - x, y, z, qw, qx, qy, qz
param7: [1, 1, 1, 3.14, 0, 0] # geometry_msgs::Pose - x, y, z, roll, pitch, yaw
param8: [2, 2, 2, 1, 0, 0, 0] # geometry_msgs::Pose - x, y, z, qw, qx, qy, qz
param9: [1, 2, 3] # geometry_msgs::Vector3 - x, y, z
param10: [25, 20, 200, 0.8] # std_msgs::ColorRGBA - r, g, b, a

```

## Testing and Linting
Expand Down
2 changes: 2 additions & 0 deletions config/example.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -8,3 +8,5 @@ example:
param6: [2, 2, 2, 1, 0, 0, 0] # Eigen::Isometry3d - x, y, z, qw, qx, qy, qz
param7: [1, 1, 1, 3.14, 0, 0] # geometry_msgs::Pose - x, y, z, roll, pitch, yaw
param8: [2, 2, 2, 1, 0, 0, 0] # geometry_msgs::Pose - x, y, z, qw, qx, qy, qz
param9: [1, 2, 3] # geometry_msgs::Vector3 - x, y, z
param10: [25, 20, 200, 0.8] # std_msgs::ColorRGBA - r, g, b, a
13 changes: 13 additions & 0 deletions include/rosparam_shortcuts/rosparam_shortcuts.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,12 @@
// geometry_msgs/Pose
#include <geometry_msgs/Pose.h>

// geometry_msgs/Vector3
#include <geometry_msgs/Vector3.h>

// std_msgs/ColorRGBA
#include <std_msgs/ColorRGBA.h>

namespace rosparam_shortcuts
{
// -------------------------------------------------------------------------------------------------
Expand Down Expand Up @@ -99,6 +105,13 @@ bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::s
bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
geometry_msgs::Pose &value);

bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
geometry_msgs::Vector3 &value);

bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name,
std_msgs::ColorRGBA &value);


/**
* \brief Output a string of values from an array for debugging
* \param array of values
Expand Down
6 changes: 6 additions & 0 deletions src/example.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,8 @@ int main(int argc, char** argv)
Eigen::Isometry3d param6;
geometry_msgs::Pose param7;
geometry_msgs::Pose param8;
geometry_msgs::Vector3 param9;
std_msgs::ColorRGBA param10;

// Load rosparams
ros::NodeHandle rpnh(nh, name);
Expand All @@ -79,6 +81,8 @@ int main(int argc, char** argv)
error += !rosparam_shortcuts::get(name, rpnh, "param6", param6); // Isometry3d param
error += !rosparam_shortcuts::get(name, rpnh, "param7", param7); // geometry_msgs::Pose param
error += !rosparam_shortcuts::get(name, rpnh, "param8", param8); // geometry_msgs::Pose param
error += !rosparam_shortcuts::get(name, rpnh, "param9", param9); // geometry_msgs::Vector3 param
error += !rosparam_shortcuts::get(name, rpnh, "param10", param10); // std_msgs::ColorRGBA param
// add more parameters here to load if desired
rosparam_shortcuts::shutdownIfError(name, error);

Expand All @@ -93,6 +97,8 @@ int main(int argc, char** argv)
ROS_INFO_STREAM_NAMED(name, "param6: Translation:\n" << param6.translation());
ROS_INFO_STREAM_NAMED(name, "param7: Pose:\n" << param7);
ROS_INFO_STREAM_NAMED(name, "param8: Pose:\n" << param8);
ROS_INFO_STREAM_NAMED(name, "param9: Vector:\n" << param9);
ROS_INFO_STREAM_NAMED(name, "param10: ColorRGBA:\n" << param10);
ROS_INFO_STREAM_NAMED(name, "Shutting down.");
ros::shutdown();

Expand Down
Loading