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.

Sign up for GitHub

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

changes so the client are only made on init and with name changes #12

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

changes so the client are only made on init and with name changes #12

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
eb487e5
changes so the client are only made on init and with name changes
Jm7752 Jan 25, 2025
f6a6c0b
remove yaw offset
Jm7752 Jan 25, 2025
7b35e8c
Update waypoint_nav_frame_ros2.cpp
Jm7752 Jan 26, 2025
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
205 changes: 76 additions & 129 deletions waypoint_navigation_plugin/src/waypoint_nav_frame_ros2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,6 @@
#include <fstream>
#include "waypoint_nav_tool_ros2.hpp"
//#include "waypoint_nav_frame.h"
#include <mav_manager_srv/srv/vec4.hpp>
#include <OGRE/OgreVector3.h>
#include <QFileDialog>
#include <boost/foreach.hpp>
Expand Down Expand Up @@ -77,6 +76,20 @@ interactive_markers::InteractiveMarkerServer* server, int* unique_ind, QWidget *
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
// quat_transform_ = Eigen::Quaternionf::Identity();
std::string srvs_name = "/"+ robot_name+"/voxblox_node/clear_map";
clear_map_client_ = node->create_client<std_srvs::srv::Empty>(srvs_name);
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/motors";
motors_client_ = node->create_client<std_srvs::srv::SetBool>(srvs_name);
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/hover";
hover_client_ = node->create_client<std_srvs::srv::Trigger>(srvs_name);
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/land";
land_client_ = node->create_client<std_srvs::srv::Trigger>(srvs_name);
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/takeoff";
takeoff_client_ = node->create_client<std_srvs::srv::Trigger>(srvs_name);
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/goTo";
go_to_client_ = node->create_client<mav_manager_srv::srv::Vec4>(srvs_name);
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/goToRelative";
go_to_relative_client_ = node->create_client<mav_manager_srv::srv::Vec4>(srvs_name);

//connect the Qt signals and slots
connect(ui_->publish_wp_button, SIGNAL(clicked()), this, SLOT(publishButtonClicked()));
Expand Down Expand Up @@ -722,107 +735,85 @@ QString WaypointFrame::getOutputTopic()

//Clear Map
void WaypointFrame::clear_map(){
auto client = node->create_client<std_srvs::srv::Empty>("/voxblox_node/clear_map");
auto request = std::make_shared<std_srvs::srv::Empty::Request>();
auto result = client->async_send_request(request);
if(rclcpp::spin_until_future_complete(node->get_node_base_interface(), result,
std::chrono::duration</*TimeRepT*/int64_t, /*TimeT*/ std::milli>(300))==
rclcpp::FutureReturnCode::SUCCESS){
RCLCPP_INFO(node->get_logger(), "Success callback Clear map");
}
else{
RCLCPP_ERROR(node->get_logger(), "Failed callback Clear map");
}
auto result = clear_map_client_->async_send_request(request);
}


//Buttons RQT MAV MANAGER
void WaypointFrame::motors_on_push_button(){
std::string srvs_name = "/"+ robot_name+"/"+mav_node_name+"/motors";
auto client = node->create_client<std_srvs::srv::SetBool>(srvs_name);
auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
request->data = true;
auto result = client->async_send_request(request);
auto result = motors_client_->async_send_request(request);
RCLCPP_INFO(node->get_logger(), "Sent Service");
if(rclcpp::spin_until_future_complete(node->get_node_base_interface(), result,
std::chrono::duration</*TimeRepT*/int64_t, /*TimeT*/ std::milli>(300))==
rclcpp::FutureReturnCode::SUCCESS){
RCLCPP_INFO(node->get_logger(), "%s Success callback", srvs_name.c_str());
}
else{
RCLCPP_ERROR(node->get_logger(), "%s Failed callback", srvs_name.c_str());
}

}

void WaypointFrame::motors_off_push_button(){
std::string srvs_name = "/"+ robot_name+"/"+mav_node_name+"/motors";
auto client = node->create_client<std_srvs::srv::SetBool>(srvs_name);
auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
request->data = false;
auto result = client->async_send_request(request);
auto result = motors_client_->async_send_request(request);
RCLCPP_INFO(node->get_logger(), "Sent Service");
if(rclcpp::spin_until_future_complete(node->get_node_base_interface(), result,
std::chrono::duration</*TimeRepT*/int64_t, /*TimeT*/ std::milli>(300))==
rclcpp::FutureReturnCode::SUCCESS){
RCLCPP_INFO(node->get_logger(), "%s Success callback", srvs_name.c_str());
}
else{
RCLCPP_ERROR(node->get_logger(), "%s Failed callback", srvs_name.c_str());
}
}

void WaypointFrame::hover_push_button(){
boost::mutex::scoped_lock lock(frame_updates_mutex_);
std::string srvs_name = "/"+ robot_name+"/"+mav_node_name+"/hover";
auto client = node->create_client<std_srvs::srv::Trigger>(srvs_name);
auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
auto result = client->async_send_request(request);
auto result = hover_client_->async_send_request(request);
RCLCPP_INFO(node->get_logger(), "Sent Service");
if(rclcpp::spin_until_future_complete(node->get_node_base_interface(), result,
std::chrono::duration</*TimeRepT*/int64_t, /*TimeT*/ std::milli>(300))==
rclcpp::FutureReturnCode::SUCCESS){
RCLCPP_INFO(node->get_logger(), "%s Success callback", srvs_name.c_str());
}
else{
RCLCPP_ERROR(node->get_logger(), "%s Failed callback", srvs_name.c_str());
}

}

void WaypointFrame::land_push_button(){
boost::mutex::scoped_lock lock(frame_updates_mutex_);
std::string srvs_name = "/"+ robot_name+"/"+mav_node_name+"/land";
auto client = node->create_client<std_srvs::srv::Trigger>(srvs_name);
auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
auto result = client->async_send_request(request);
auto result = land_client_->async_send_request(request);
RCLCPP_INFO(node->get_logger(), "Sent Service");
if(rclcpp::spin_until_future_complete(node->get_node_base_interface(), result,
std::chrono::duration</*TimeRepT*/int64_t, /*TimeT*/ std::milli>(300))==
rclcpp::FutureReturnCode::SUCCESS){
RCLCPP_INFO(node->get_logger(), "%s Success callback", srvs_name.c_str());
}
else{
RCLCPP_ERROR(node->get_logger(), "%s Failed callback", srvs_name.c_str());
}
}

void WaypointFrame::takeoff_push_button(){
boost::mutex::scoped_lock lock(frame_updates_mutex_);
std::string srvs_name = "/"+ robot_name+"/"+mav_node_name+"/takeoff";
//ros::ServiceClient client = nh.serviceClient<std_srvs::Trigger>(srvs_name);
auto client = node->create_client<std_srvs::srv::Trigger>(srvs_name);
auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
auto result = client->async_send_request(request);
auto result = takeoff_client_->async_send_request(request);
RCLCPP_INFO(node->get_logger(), "Sent Service");
}

void WaypointFrame::goto_push_button(){

Eigen::Vector3f goalPos(ui_->x_doubleSpinBox_gt->value(),
ui_->y_doubleSpinBox_gt->value(),
ui_->z_doubleSpinBox_gt->value());

goalPos = quat_transform_ * goalPos;
double yaw = ui_->yaw_doubleSpinBox_gt->value();

boost::mutex::scoped_lock lock(frame_updates_mutex_);
if(relative_){
auto request = std::make_shared<mav_manager_srv::srv::Vec4::Request>();
request->goal[0] = goalPos.x();
request->goal[1] = goalPos.y();
request->goal[2] = goalPos.z();
request->goal[3] = yaw;
auto result = go_to_relative_client_->async_send_request(request);
}
else{
auto request = std::make_shared<mav_manager_srv::srv::Vec4::Request>();
request->goal[0] = goalPos.x();
request->goal[1] = goalPos.y();
request->goal[2] = goalPos.z();
request->goal[3] = yaw;
auto result = go_to_client_->async_send_request(request);
}
RCLCPP_INFO(node->get_logger(), "Sent Service");
}

void WaypointFrame::goHome_push_button(){
boost::mutex::scoped_lock lock(frame_updates_mutex_);
auto request = std::make_shared<mav_manager_srv::srv::Vec4::Request>();
request->goal[0] = 0.0;
request->goal[1] = 0.0;
request->goal[2] = 0.5;
request->goal[3] = 0.0;
auto result = go_to_client_->async_send_request(request);
RCLCPP_INFO(node->get_logger(), "Sent Service");
if(rclcpp::spin_until_future_complete(node->get_node_base_interface(), result,
std::chrono::duration</*TimeRepT*/int64_t, /*TimeT*/ std::milli>(300))==
rclcpp::FutureReturnCode::SUCCESS){
RCLCPP_INFO(node->get_logger(), "%s Success callback", srvs_name.c_str());
}
else{
RCLCPP_ERROR(node->get_logger(), "%s Failed callback", srvs_name.c_str());
}
}

void WaypointFrame::tf2_callback(){
Expand Down Expand Up @@ -858,44 +849,6 @@ void WaypointFrame::tf2_callback(){
}
}

void WaypointFrame::goto_push_button(){

Eigen::Vector3f goalPos(ui_->x_doubleSpinBox_gt->value(),
ui_->y_doubleSpinBox_gt->value(),
ui_->z_doubleSpinBox_gt->value());

goalPos = quat_transform_ * goalPos;
double yaw = ui_->yaw_doubleSpinBox_gt->value();

boost::mutex::scoped_lock lock(frame_updates_mutex_);
std::string srvs_name;// = "/"+ robot_name+"/"+mav_node_name+"/goTo";
if(relative_){
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/goToRelative";
}
else{
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/goTo";
yaw-=yaw_init_;
}
//ros::ServiceClient client = nh.serviceClient<std_srvs::Trigger>(srvs_name);
auto client = node->create_client<mav_manager_srv::srv::Vec4>(srvs_name);
auto request = std::make_shared<mav_manager_srv::srv::Vec4::Request>();
request->goal[0] = goalPos.x();
request->goal[1] = goalPos.y();
request->goal[2] = goalPos.z();
request->goal[3] = yaw;

auto result = client->async_send_request(request);
RCLCPP_INFO(node->get_logger(), "Sent Service");
if(rclcpp::spin_until_future_complete(node->get_node_base_interface(), result,
std::chrono::duration</*TimeRepT*/int64_t, /*TimeT*/ std::milli>(300))==
rclcpp::FutureReturnCode::SUCCESS){
RCLCPP_INFO(node->get_logger(), "%s Success callback", srvs_name.c_str());
}
else{
RCLCPP_ERROR(node->get_logger(), "%s Failed callback", srvs_name.c_str());
}
}

void WaypointFrame::relativeChanged(int b){
boost::mutex::scoped_lock lock(frame_updates_mutex_);
if (b ==2){
Expand Down Expand Up @@ -924,6 +877,22 @@ void WaypointFrame::robotChanged(){
boost::mutex::scoped_lock lock(frame_updates_mutex_);
QString new_frame = ui_->robot_name_line_edit->text();
robot_name = new_frame.toStdString();
std::string srvs_name = "/"+ robot_name+"/voxblox_node/clear_map";
clear_map_client_ = node->create_client<std_srvs::srv::Empty>(srvs_name);
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/motors";
motors_client_ = node->create_client<std_srvs::srv::SetBool>(srvs_name);
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/hover";
hover_client_ = node->create_client<std_srvs::srv::Trigger>(srvs_name);
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/land";
land_client_ = node->create_client<std_srvs::srv::Trigger>(srvs_name);
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/takeoff";
takeoff_client_ = node->create_client<std_srvs::srv::Trigger>(srvs_name);
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/goTo";
go_to_client_ = node->create_client<mav_manager_srv::srv::Vec4>(srvs_name);
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/goToRelative";
go_to_relative_client_ = node->create_client<mav_manager_srv::srv::Vec4>(srvs_name);


}

void WaypointFrame::serviceChanged(){
Expand All @@ -932,29 +901,7 @@ void WaypointFrame::serviceChanged(){
mav_node_name = new_frame.toStdString();
}

void WaypointFrame::goHome_push_button(){
boost::mutex::scoped_lock lock(frame_updates_mutex_);
std::string srvs_name;// = "/"+ robot_name+"/"+mav_node_name+"/goTo";
srvs_name = "/"+ robot_name+"/"+mav_node_name+"/goTo";
//ros::ServiceClient client = nh.serviceClient<std_srvs::Trigger>(srvs_name);
auto client = node->create_client<mav_manager_srv::srv::Vec4>(srvs_name);
auto request = std::make_shared<mav_manager_srv::srv::Vec4::Request>();
request->goal[0] = 0.0;
request->goal[1] = 0.0;
request->goal[2] = 0.5;
request->goal[3] = 0.0;
auto result = client->async_send_request(request);
RCLCPP_INFO(node->get_logger(), "Sent Service");
if(rclcpp::spin_until_future_complete(node->get_node_base_interface(), result,
std::chrono::duration</*TimeRepT*/int64_t, /*TimeT*/ std::milli>(300))==
rclcpp::FutureReturnCode::SUCCESS){
RCLCPP_INFO(node->get_logger(), "%s Success callback", srvs_name.c_str());
}
else{
RCLCPP_ERROR(node->get_logger(), "%s Failed callback", srvs_name.c_str());
}

}

Eigen::Quaternionf WaypointFrame::getQuatTransform(){
return (quat_transform_);
Expand Down
9 changes: 8 additions & 1 deletion waypoint_navigation_plugin/src/waypoint_nav_frame_ros2.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,7 @@
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include <Eigen/Geometry>
#include <mav_manager_srv/srv/vec4.hpp>

typedef struct {
int derivOrder, vertexNum;
Expand Down Expand Up @@ -215,7 +216,13 @@ private Q_SLOTS:
rclcpp::TimerBase::SharedPtr timer_{nullptr};
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;

rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr motors_client_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr land_client_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr takeoff_client_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr hover_client_;
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr clear_map_client_;
rclcpp::Client<mav_manager_srv::srv::Vec4>::SharedPtr go_to_client_;
rclcpp::Client<mav_manager_srv::srv::Vec4>::SharedPtr go_to_relative_client_;
//rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_ path_listen_;
//rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_ vel_listen_;
//rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_ acc_listen_;
Expand Down
1 change: 0 additions & 1 deletion waypoint_navigation_plugin/src/waypoint_nav_tool_ros2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,6 @@ WaypointNavTool::~WaypointNavTool()
{
scene_manager_->destroySceneNode(sn_it->second);
}

delete frame_;
delete frame_dock_;
delete moving_flag_node_;
Expand Down