diff --git a/cpp/waypoint_flier/CMakeLists.txt b/cpp/waypoint_flier/CMakeLists.txt
index c5b6a59..f98c098 100644
--- a/cpp/waypoint_flier/CMakeLists.txt
+++ b/cpp/waypoint_flier/CMakeLists.txt
@@ -15,7 +15,7 @@ SET(CATKIN_DEPENDENCIES
)
SET(LIBRARIES
- WaypointFlier
+ ExampleWaypointFlier
)
find_package(catkin REQUIRED COMPONENTS
@@ -43,17 +43,17 @@ include_directories(
)
## Declare a C++ library
-add_library(WaypointFlier
- src/waypoint_flier.cpp
+add_library(ExampleWaypointFlier
+ src/example_waypoint_flier.cpp
)
## Add configure headers for dynamic reconfigure
-add_dependencies(WaypointFlier
+add_dependencies(ExampleWaypointFlier
${PROJECT_NAME}_gencfg
)
## Specify libraries to link a library or executable target against
-target_link_libraries(WaypointFlier
+target_link_libraries(ExampleWaypointFlier
${catkin_LIBRARIES}
${Eigen_LIBRARIES}
)
diff --git a/cpp/waypoint_flier/README.md b/cpp/waypoint_flier/README.md
index b80af73..c80b859 100644
--- a/cpp/waypoint_flier/README.md
+++ b/cpp/waypoint_flier/README.md
@@ -1,4 +1,4 @@
-# WaypointFlier ROS example
+# ExampleWaypointFlier ROS example
This package was created as an example of how to write ROS nodelets.
The package is written in C++ and features custom MRS libraries and msgs.
@@ -58,7 +58,7 @@ Also check out our general [C++ good/bad coding practices tutorial](https://ctu-
- `sub_odom_uav_` - member subscriber to uav odometry msg type
- `pub_reference_` - member publisher of reference msg type
- `srv_server_start_waypoints_following_` - member service server for starting following of waypoints
- - `WaypointFlier::callbackTimerCheckSubscribers()` - callback of timer which checks subscribers
+ - `ExampleWaypointFlier::callbackTimerCheckSubscribers()` - callback of timer which checks subscribers
- `mutex_odom_uav_` - mutex locking access to variable containing odometry of the UAV
### Good practices
diff --git a/cpp/waypoint_flier/config/dynparam.cfg b/cpp/waypoint_flier/config/dynparam.cfg
index 47a8c43..2d1839c 100755
--- a/cpp/waypoint_flier/config/dynparam.cfg
+++ b/cpp/waypoint_flier/config/dynparam.cfg
@@ -7,7 +7,7 @@ roslib.load_manifest(PACKAGE)
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator();
-params = gen.add_group("waypoint_flier parameters");
+params = gen.add_group("example_waypoint_flier parameters");
params.add("waypoint_idle_time", double_t, 1, "The time to wait before flying to the next waypoint", 1, 0, 60);
diff --git a/cpp/waypoint_flier/config/waypoint_flier.yaml b/cpp/waypoint_flier/config/example_waypoint_flier.yaml
similarity index 100%
rename from cpp/waypoint_flier/config/waypoint_flier.yaml
rename to cpp/waypoint_flier/config/example_waypoint_flier.yaml
diff --git a/cpp/waypoint_flier/launch/waypoint_flier.launch b/cpp/waypoint_flier/launch/example_waypoint_flier.launch
similarity index 79%
rename from cpp/waypoint_flier/launch/waypoint_flier.launch
rename to cpp/waypoint_flier/launch/example_waypoint_flier.launch
index 370edad..3b7e3fb 100644
--- a/cpp/waypoint_flier/launch/waypoint_flier.launch
+++ b/cpp/waypoint_flier/launch/example_waypoint_flier.launch
@@ -10,23 +10,23 @@
-
+
-
+
-
-
+
+
-
+
diff --git a/cpp/waypoint_flier/nodelets.xml b/cpp/waypoint_flier/nodelets.xml
index 13d2385..61258c9 100644
--- a/cpp/waypoint_flier/nodelets.xml
+++ b/cpp/waypoint_flier/nodelets.xml
@@ -1,5 +1,5 @@
-
-
- WaypointFlier nodelet
+
+
+ ExampleWaypointFlier nodelet
diff --git a/cpp/waypoint_flier/src/waypoint_flier.cpp b/cpp/waypoint_flier/src/example_waypoint_flier.cpp
similarity index 78%
rename from cpp/waypoint_flier/src/waypoint_flier.cpp
rename to cpp/waypoint_flier/src/example_waypoint_flier.cpp
index 84b9462..f8cd599 100644
--- a/cpp/waypoint_flier/src/waypoint_flier.cpp
+++ b/cpp/waypoint_flier/src/example_waypoint_flier.cpp
@@ -55,9 +55,9 @@ using vec3_t = mrs_lib::geometry::vec_t<3>;
namespace example_waypoint_flier
{
-/* class WaypointFlier //{ */
+/* class ExampleWaypointFlier //{ */
-class WaypointFlier : public nodelet::Nodelet {
+class ExampleWaypointFlier : public nodelet::Nodelet {
public:
/* onInit() is called when nodelet is launched (similar to main() in regular node) */
@@ -153,7 +153,7 @@ class WaypointFlier : public nodelet::Nodelet {
/* onInit() //{ */
-void WaypointFlier::onInit() {
+void ExampleWaypointFlier::onInit() {
// | ---------------- set my booleans to false ---------------- |
// but remember, always set them to their default value in the header file
@@ -171,7 +171,7 @@ void WaypointFlier::onInit() {
// | ------------------- load ros parameters ------------------ |
/* (mrs_lib implementation checks whether the parameter was loaded or not) */
- mrs_lib::ParamLoader param_loader(nh, "WaypointFlier");
+ mrs_lib::ParamLoader param_loader(nh, "ExampleWaypointFlier");
param_loader.loadParam("uav_name", _uav_name_);
param_loader.loadParam("land_at_the_end", _land_end_);
@@ -190,15 +190,15 @@ void WaypointFlier::onInit() {
waypoints_loaded_ = true;
idx_current_waypoint_ = 0;
c_loop_ = 0;
- ROS_INFO_STREAM_ONCE("[WaypointFlier]: " << n_waypoints_ << " waypoints loaded");
- ROS_INFO_STREAM_ONCE("[WaypointFlier]: " << _n_loops_ << " loops requested");
+ ROS_INFO_STREAM_ONCE("[ExampleWaypointFlier]: " << n_waypoints_ << " waypoints loaded");
+ ROS_INFO_STREAM_ONCE("[ExampleWaypointFlier]: " << _n_loops_ << " loops requested");
/* load offset of all waypoints as a static matrix from config file */
param_loader.loadMatrixStatic("offset", _offset_, 1, 4);
offsetPoints(waypoints_, _offset_);
if (!param_loader.loadedSuccessfully()) {
- ROS_ERROR("[WaypointFlier]: failed to load non-optional parameters!");
+ ROS_ERROR("[ExampleWaypointFlier]: failed to load non-optional parameters!");
ros::shutdown();
}
@@ -206,7 +206,7 @@ void WaypointFlier::onInit() {
mrs_lib::SubscribeHandlerOptions shopts;
shopts.nh = nh;
- shopts.node_name = "WaypointFlier";
+ shopts.node_name = "ExampleWaypointFlier";
shopts.no_message_timeout = ros::Duration(1.0);
shopts.threadsafe = true;
shopts.autostart = true;
@@ -215,7 +215,7 @@ void WaypointFlier::onInit() {
sh_odometry_ = mrs_lib::SubscribeHandler(shopts, "odom_uav_in");
sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "control_manager_diagnostics_in",
- &WaypointFlier::callbackControlManagerDiag, this);
+ &ExampleWaypointFlier::callbackControlManagerDiag, this);
// | ------------------ initialize publishers ----------------- |
@@ -224,18 +224,18 @@ void WaypointFlier::onInit() {
// | -------------------- initialize timers ------------------- |
- timer_publish_dist_to_waypoint_ = nh.createTimer(ros::Rate(_rate_timer_publish_dist_to_waypoint_), &WaypointFlier::timerPublishDistToWaypoint, this);
+ timer_publish_dist_to_waypoint_ = nh.createTimer(ros::Rate(_rate_timer_publish_dist_to_waypoint_), &ExampleWaypointFlier::timerPublishDistToWaypoint, this);
- timer_check_subscribers_ = nh.createTimer(ros::Rate(_rate_timer_check_subscribers_), &WaypointFlier::timerCheckSubscribers, this);
+ timer_check_subscribers_ = nh.createTimer(ros::Rate(_rate_timer_check_subscribers_), &ExampleWaypointFlier::timerCheckSubscribers, this);
// you can disable autostarting of the timer by the last argument
- timer_publisher_reference_ = nh.createTimer(ros::Rate(_rate_timer_publisher_reference_), &WaypointFlier::timerPublishSetReference, this, false, false);
+ timer_publisher_reference_ = nh.createTimer(ros::Rate(_rate_timer_publisher_reference_), &ExampleWaypointFlier::timerPublishSetReference, this, false, false);
// | --------------- initialize service servers --------------- |
- srv_server_start_waypoints_following_ = nh.advertiseService("start_waypoints_following_in", &WaypointFlier::callbackStartWaypointFollowing, this);
- srv_server_stop_waypoints_following_ = nh.advertiseService("stop_waypoints_following_in", &WaypointFlier::callbackStopWaypointFollowing, this);
- srv_server_fly_to_first_waypoint_ = nh.advertiseService("fly_to_first_waypoint_in", &WaypointFlier::callbackFlyToFirstWaypoint, this);
+ srv_server_start_waypoints_following_ = nh.advertiseService("start_waypoints_following_in", &ExampleWaypointFlier::callbackStartWaypointFollowing, this);
+ srv_server_stop_waypoints_following_ = nh.advertiseService("stop_waypoints_following_in", &ExampleWaypointFlier::callbackStopWaypointFollowing, this);
+ srv_server_fly_to_first_waypoint_ = nh.advertiseService("fly_to_first_waypoint_in", &ExampleWaypointFlier::callbackFlyToFirstWaypoint, this);
// | --------------- initialize service clients --------------- |
@@ -244,7 +244,7 @@ void WaypointFlier::onInit() {
// | ---------- initialize dynamic reconfigure server --------- |
reconfigure_server_.reset(new ReconfigureServer(mutex_dynamic_reconfigure_, nh));
- ReconfigureServer::CallbackType f = boost::bind(&WaypointFlier::callbackDynamicReconfigure, this, _1, _2);
+ ReconfigureServer::CallbackType f = boost::bind(&ExampleWaypointFlier::callbackDynamicReconfigure, this, _1, _2);
reconfigure_server_->setCallback(f);
/* set the default value of dynamic reconfigure server to the value of parameter with the same name */
@@ -254,7 +254,7 @@ void WaypointFlier::onInit() {
}
reconfigure_server_->updateConfig(last_drs_config_);
- ROS_INFO_ONCE("[WaypointFlier]: initialized");
+ ROS_INFO_ONCE("[ExampleWaypointFlier]: initialized");
is_initialized_ = true;
}
@@ -265,14 +265,14 @@ void WaypointFlier::onInit() {
/* callbackControlManagerDiag() //{ */
-void WaypointFlier::callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnostics::ConstPtr diagnostics) {
+void ExampleWaypointFlier::callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnostics::ConstPtr diagnostics) {
/* do not continue if the nodelet is not initialized */
if (!is_initialized_) {
return;
}
- ROS_INFO_ONCE("[WaypointFlier]: Received first control manager diagnostics msg");
+ ROS_INFO_ONCE("[ExampleWaypointFlier]: Received first control manager diagnostics msg");
// get the variable under the mutex
mrs_msgs::Reference current_waypoint = mrs_lib::get_mutexed(mutex_current_waypoint_, current_waypoint_);
@@ -281,21 +281,21 @@ void WaypointFlier::callbackControlManagerDiag(const mrs_msgs::ControlManagerDia
geometry_msgs::Pose current_pose = mrs_lib::getPose(sh_odometry_.getMsg());
double dist = distance(current_waypoint, current_pose);
- ROS_INFO("[WaypointFlier]: Distance to waypoint: %.2f", dist);
+ ROS_INFO("[ExampleWaypointFlier]: Distance to waypoint: %.2f", dist);
if (have_goal_ && dist < 0.1 && !diagnostics->tracker_status.have_goal) {
- ROS_INFO("[WaypointFlier]: Waypoint reached.");
+ ROS_INFO("[ExampleWaypointFlier]: Waypoint reached.");
have_goal_ = false;
/* start idling at the reached waypoint */
is_idling_ = true;
ros::NodeHandle nh("~");
- timer_idling_ = nh.createTimer(ros::Duration(_waypoint_idle_time_), &WaypointFlier::timerIdling, this,
+ timer_idling_ = nh.createTimer(ros::Duration(_waypoint_idle_time_), &ExampleWaypointFlier::timerIdling, this,
true); // the last boolean argument makes the timer run only once
- ROS_INFO("[WaypointFlier]: Idling for %.2f seconds.", _waypoint_idle_time_);
+ ROS_INFO("[ExampleWaypointFlier]: Idling for %.2f seconds.", _waypoint_idle_time_);
}
}
@@ -305,7 +305,7 @@ void WaypointFlier::callbackControlManagerDiag(const mrs_msgs::ControlManagerDia
/* timerPublishSetReference() //{ */
-void WaypointFlier::timerPublishSetReference([[maybe_unused]] const ros::TimerEvent& te) {
+void ExampleWaypointFlier::timerPublishSetReference([[maybe_unused]] const ros::TimerEvent& te) {
if (!is_initialized_) {
return;
@@ -326,24 +326,24 @@ void WaypointFlier::timerPublishSetReference([[maybe_unused]] const ros::TimerEv
c_loop_++;
- ROS_INFO("[WaypointFlier]: Finished loop %d/%d", c_loop_, _n_loops_);
+ ROS_INFO("[ExampleWaypointFlier]: Finished loop %d/%d", c_loop_, _n_loops_);
if (c_loop_ >= _n_loops_) {
- ROS_INFO("[WaypointFlier]: Finished %d loops of %d waypoints.", _n_loops_, n_waypoints_);
+ ROS_INFO("[ExampleWaypointFlier]: Finished %d loops of %d waypoints.", _n_loops_, n_waypoints_);
if (_land_end_) {
- ROS_INFO("[WaypointFlier]: Calling land service.");
+ ROS_INFO("[ExampleWaypointFlier]: Calling land service.");
std_srvs::Trigger srv_land_call;
srv_client_land_.call(srv_land_call);
}
- ROS_INFO("[WaypointFlier]: Shutting down.");
+ ROS_INFO("[ExampleWaypointFlier]: Shutting down.");
ros::shutdown();
return;
} else {
- ROS_INFO("[WaypointFlier]: Starting loop %d/%d", c_loop_ + 1, _n_loops_);
+ ROS_INFO("[ExampleWaypointFlier]: Starting loop %d/%d", c_loop_ + 1, _n_loops_);
idx_current_waypoint_ = 0;
}
}
@@ -360,7 +360,7 @@ void WaypointFlier::timerPublishSetReference([[maybe_unused]] const ros::TimerEv
// set the variable under the mutex
mrs_lib::set_mutexed(mutex_current_waypoint_, waypoints_.at(idx_current_waypoint_), current_waypoint_);
- ROS_INFO("[WaypointFlier]: Flying to waypoint %d: x: %.2f y: %.2f z: %.2f heading: %.2f", idx_current_waypoint_ + 1, new_waypoint.reference.position.x,
+ ROS_INFO("[ExampleWaypointFlier]: Flying to waypoint %d: x: %.2f y: %.2f z: %.2f heading: %.2f", idx_current_waypoint_ + 1, new_waypoint.reference.position.x,
new_waypoint.reference.position.y, new_waypoint.reference.position.z, new_waypoint.reference.heading);
try {
@@ -379,7 +379,7 @@ void WaypointFlier::timerPublishSetReference([[maybe_unused]] const ros::TimerEv
/* timerPublishDistToWaypoint() //{ */
-void WaypointFlier::timerPublishDistToWaypoint([[maybe_unused]] const ros::TimerEvent& te) {
+void ExampleWaypointFlier::timerPublishDistToWaypoint([[maybe_unused]] const ros::TimerEvent& te) {
if (!is_initialized_) {
return;
@@ -402,7 +402,7 @@ void WaypointFlier::timerPublishDistToWaypoint([[maybe_unused]] const ros::Timer
geometry_msgs::Pose current_pose = mrs_lib::getPose(sh_odometry_.getMsg());
double dist = distance(current_waypoint, current_pose);
- ROS_INFO("[WaypointFlier]: Distance to waypoint: %.2f", dist);
+ ROS_INFO("[ExampleWaypointFlier]: Distance to waypoint: %.2f", dist);
mrs_msgs::Float64Stamped dist_msg;
// it is important to set the frame id correctly !!
@@ -422,18 +422,18 @@ void WaypointFlier::timerPublishDistToWaypoint([[maybe_unused]] const ros::Timer
/* timerCheckSubscribers() //{ */
-void WaypointFlier::timerCheckSubscribers([[maybe_unused]] const ros::TimerEvent& te) {
+void ExampleWaypointFlier::timerCheckSubscribers([[maybe_unused]] const ros::TimerEvent& te) {
if (!is_initialized_) {
return;
}
if (!sh_odometry_.hasMsg()) {
- ROS_WARN_THROTTLE(1.0, "[WaypointFlier]: Not received uav odom msg since node launch.");
+ ROS_WARN_THROTTLE(1.0, "[ExampleWaypointFlier]: Not received uav odom msg since node launch.");
}
if (!sh_control_manager_diag_.hasMsg()) {
- ROS_WARN_THROTTLE(1.0, "[WaypointFlier]: Not received tracker diagnostics msg since node launch.");
+ ROS_WARN_THROTTLE(1.0, "[ExampleWaypointFlier]: Not received tracker diagnostics msg since node launch.");
}
}
@@ -441,9 +441,9 @@ void WaypointFlier::timerCheckSubscribers([[maybe_unused]] const ros::TimerEvent
/* timerIdling() //{ */
-void WaypointFlier::timerIdling([[maybe_unused]] const ros::TimerEvent& te) {
+void ExampleWaypointFlier::timerIdling([[maybe_unused]] const ros::TimerEvent& te) {
- ROS_INFO("[WaypointFlier]: Idling finished");
+ ROS_INFO("[ExampleWaypointFlier]: Idling finished");
is_idling_ = false;
}
@@ -453,13 +453,13 @@ void WaypointFlier::timerIdling([[maybe_unused]] const ros::TimerEvent& te) {
/* //{ callbackStartWaypointFollowing() */
-bool WaypointFlier::callbackStartWaypointFollowing([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+bool ExampleWaypointFlier::callbackStartWaypointFollowing([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
if (!is_initialized_) {
res.success = false;
res.message = "Waypoint flier not initialized!";
- ROS_WARN("[WaypointFlier]: Cannot start waypoint following, nodelet is not initialized.");
+ ROS_WARN("[ExampleWaypointFlier]: Cannot start waypoint following, nodelet is not initialized.");
return true;
}
@@ -467,14 +467,14 @@ bool WaypointFlier::callbackStartWaypointFollowing([[maybe_unused]] std_srvs::Tr
timer_publisher_reference_.start();
- ROS_INFO("[WaypointFlier]: Starting waypoint following.");
+ ROS_INFO("[ExampleWaypointFlier]: Starting waypoint following.");
res.success = true;
res.message = "Starting waypoint following.";
} else {
- ROS_WARN("[WaypointFlier]: Cannot start waypoint following, waypoints are not set.");
+ ROS_WARN("[ExampleWaypointFlier]: Cannot start waypoint following, waypoints are not set.");
res.success = false;
res.message = "Waypoints not set.";
}
@@ -486,19 +486,19 @@ bool WaypointFlier::callbackStartWaypointFollowing([[maybe_unused]] std_srvs::Tr
/* //{ callbackStopWaypointFollowing() */
-bool WaypointFlier::callbackStopWaypointFollowing([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+bool ExampleWaypointFlier::callbackStopWaypointFollowing([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
if (!is_initialized_) {
res.success = false;
res.message = "Waypoint flier not initialized!";
- ROS_WARN("[WaypointFlier]: Cannot stop waypoint following, nodelet is not initialized.");
+ ROS_WARN("[ExampleWaypointFlier]: Cannot stop waypoint following, nodelet is not initialized.");
return true;
}
timer_publisher_reference_.stop();
- ROS_INFO("[WaypointFlier]: Waypoint following stopped.");
+ ROS_INFO("[ExampleWaypointFlier]: Waypoint following stopped.");
res.success = true;
res.message = "Waypoint following stopped.";
@@ -510,13 +510,13 @@ bool WaypointFlier::callbackStopWaypointFollowing([[maybe_unused]] std_srvs::Tri
/* //{ callbackFlyToFirstWaypoint() */
-bool WaypointFlier::callbackFlyToFirstWaypoint([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+bool ExampleWaypointFlier::callbackFlyToFirstWaypoint([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
if (!is_initialized_) {
res.success = false;
res.message = "Waypoint flier not initialized!";
- ROS_WARN("[WaypointFlier]: Cannot start waypoint following, nodelet is not initialized.");
+ ROS_WARN("[ExampleWaypointFlier]: Cannot start waypoint following, nodelet is not initialized.");
return true;
}
@@ -551,14 +551,14 @@ bool WaypointFlier::callbackFlyToFirstWaypoint([[maybe_unused]] std_srvs::Trigge
ss << "Flying to first waypoint: x: " << new_waypoint.reference.position.x << ", y: " << new_waypoint.reference.position.y
<< ", z: " << new_waypoint.reference.position.z << ", heading: " << new_waypoint.reference.heading;
- ROS_INFO_STREAM_THROTTLE(1.0, "[WaypointFlier]: " << ss.str());
+ ROS_INFO_STREAM_THROTTLE(1.0, "[ExampleWaypointFlier]: " << ss.str());
res.success = true;
res.message = ss.str();
} else {
- ROS_WARN("[WaypointFlier]: Cannot fly to first waypoint, waypoints not loaded!");
+ ROS_WARN("[ExampleWaypointFlier]: Cannot fly to first waypoint, waypoints not loaded!");
res.success = false;
res.message = "Waypoints not loaded";
@@ -573,13 +573,13 @@ bool WaypointFlier::callbackFlyToFirstWaypoint([[maybe_unused]] std_srvs::Trigge
/* //{ callbackDynamicReconfigure() */
-void WaypointFlier::callbackDynamicReconfigure([[maybe_unused]] Config& config, [[maybe_unused]] uint32_t level) {
+void ExampleWaypointFlier::callbackDynamicReconfigure([[maybe_unused]] Config& config, [[maybe_unused]] uint32_t level) {
if (!is_initialized_)
return;
ROS_INFO(
- "[WaypointFlier]:"
+ "[ExampleWaypointFlier]:"
"Reconfigure Request: "
"Waypoint idle time: %.2f",
config.waypoint_idle_time);
@@ -597,7 +597,7 @@ void WaypointFlier::callbackDynamicReconfigure([[maybe_unused]] Config& config,
/* matrixToPoints() //{ */
-std::vector WaypointFlier::matrixToPoints(const Eigen::MatrixXd& matrix) {
+std::vector ExampleWaypointFlier::matrixToPoints(const Eigen::MatrixXd& matrix) {
std::vector points;
@@ -619,7 +619,7 @@ std::vector WaypointFlier::matrixToPoints(const Eigen::Matr
/* offsetPoints() //{ */
-void WaypointFlier::offsetPoints(std::vector& points, const Eigen::MatrixXd& offset) {
+void ExampleWaypointFlier::offsetPoints(std::vector& points, const Eigen::MatrixXd& offset) {
for (size_t i = 0; i < points.size(); i++) {
@@ -634,7 +634,7 @@ void WaypointFlier::offsetPoints(std::vector& points, const
/* distance() //{ */
-double WaypointFlier::distance(const mrs_msgs::Reference& waypoint, const geometry_msgs::Pose& pose) {
+double ExampleWaypointFlier::distance(const mrs_msgs::Reference& waypoint, const geometry_msgs::Pose& pose) {
return mrs_lib::geometry::dist(vec3_t(waypoint.position.x, waypoint.position.y, waypoint.position.z),
vec3_t(pose.position.x, pose.position.y, pose.position.z));
@@ -646,4 +646,4 @@ double WaypointFlier::distance(const mrs_msgs::Reference& waypoint, const geomet
/* every nodelet must include macros which export the class as a nodelet plugin */
#include
-PLUGINLIB_EXPORT_CLASS(example_waypoint_flier::WaypointFlier, nodelet::Nodelet);
+PLUGINLIB_EXPORT_CLASS(example_waypoint_flier::ExampleWaypointFlier, nodelet::Nodelet);
diff --git a/cpp/waypoint_flier/tmux/session.yml b/cpp/waypoint_flier/tmux/session.yml
index daddfeb..198a4de 100644
--- a/cpp/waypoint_flier/tmux/session.yml
+++ b/cpp/waypoint_flier/tmux/session.yml
@@ -6,7 +6,7 @@ attach: false
tmux_options: -f /etc/ctu-mrs/tmux.conf
# you can modify these
pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500
-startup_window: waypointflier
+startup_window: example_waypoint_flier
windows:
- roscore:
layout: tiled
@@ -37,13 +37,13 @@ windows:
custom_config:=./config/custom_config.yaml
world_config:=./config/world_config.yaml
network_config:=./config/network_config.yaml
- - waypointflier:
+ - example_waypoint_flier:
layout: tiled
panes:
- - waitForControl; roslaunch example_waypoint_flier waypoint_flier.launch
- - 'history -s rosservice call /$UAV_NAME/waypoint_flier/fly_to_first_waypoint'
- - 'history -s rosservice call /$UAV_NAME/waypoint_flier/start_waypoints_following'
- - 'history -s rosservice call /$UAV_NAME/waypoint_flier/stop_waypoints_following'
+ - waitForControl; roslaunch example_waypoint_flier example_waypoint_flier.launch
+ - 'history -s rosservice call /$UAV_NAME/example_waypoint_flier/fly_to_first_waypoint'
+ - 'history -s rosservice call /$UAV_NAME/example_waypoint_flier/start_waypoints_following'
+ - 'history -s rosservice call /$UAV_NAME/example_waypoint_flier/stop_waypoints_following'
- rviz:
layout: tiled
panes: