diff --git a/interfaces/kr_crazyflie_interface/CMakeLists.txt b/interfaces/kr_crazyflie_interface/CMakeLists.txt index 9029cf6f..594c419f 100644 --- a/interfaces/kr_crazyflie_interface/CMakeLists.txt +++ b/interfaces/kr_crazyflie_interface/CMakeLists.txt @@ -1,48 +1,35 @@ cmake_minimum_required(VERSION 3.10) project(kr_crazyflie_interface) -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) - -find_package( - catkin REQUIRED - COMPONENTS roscpp - nav_msgs - geometry_msgs - kr_mav_msgs - nodelet) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(kr_mav_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(Eigen3 REQUIRED) -include_directories(${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) - -catkin_package( - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - roscpp - nav_msgs - geometry_msgs - kr_mav_msgs - nodelet - DEPENDS - EIGEN3) +add_library(${PROJECT_NAME} SHARED src/so3cmd_to_crazyflie_component.cpp) +ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_components kr_mav_msgs nav_msgs geometry_msgs) +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) +rclcpp_components_register_nodes(${PROJECT_NAME} "SO3CmdToCrazyflie") -add_library(${PROJECT_NAME} src/so3cmd_to_crazyflie_nodelet.cpp) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) -install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) -install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/interfaces/kr_crazyflie_interface/config/crazyflie.yaml b/interfaces/kr_crazyflie_interface/config/crazyflie.yaml index 80f41c9d..b2e0bf59 100644 --- a/interfaces/kr_crazyflie_interface/config/crazyflie.yaml +++ b/interfaces/kr_crazyflie_interface/config/crazyflie.yaml @@ -1,7 +1,9 @@ -kp_yaw_rate: 0.1 -c1: -0.6709 -c2: 0.1932 -c3: 13.0652 -so3_cmd_timeout: 0.1 -thrust_pwm_max: 60000 -thrust_pwm_min: 20000 \ No newline at end of file +so3cmd_to_crazyflie: + ros__parameters: + kp_yaw_rate: 0.1 + c1: -0.6709 + c2: 0.1932 + c3: 13.0652 + so3_cmd_timeout: 0.1 + thrust_pwm_max: 60000 + thrust_pwm_min: 20000 diff --git a/interfaces/kr_crazyflie_interface/launch/test.launch b/interfaces/kr_crazyflie_interface/launch/test.launch deleted file mode 100644 index 582852a0..00000000 --- a/interfaces/kr_crazyflie_interface/launch/test.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - diff --git a/interfaces/kr_crazyflie_interface/launch/test.launch.py b/interfaces/kr_crazyflie_interface/launch/test.launch.py new file mode 100644 index 00000000..fa802026 --- /dev/null +++ b/interfaces/kr_crazyflie_interface/launch/test.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + + robot_ns = LaunchConfiguration('robot') + odom_topic = LaunchConfiguration('odom') + so3_cmd_topic = LaunchConfiguration('so3_cmd') + + robot_arg = DeclareLaunchArgument( + 'robot', default_value='' + ) + odom_arg = DeclareLaunchArgument( + 'odom', default_value='odom' + ) + so3_cmd_arg = DeclareLaunchArgument( + 'so3_cmd', default_value='so3_cmd' + ) + + # Path to the configuration file + config_file = FindPackageShare('kr_crazyflie_interface').find('kr_crazyflie_interface') + '/config/crazyflie.yaml' + + # Component configuration + so3cmd_to_crazyflie_component = ComposableNodeContainer( + name="so3_container", + namespace=LaunchConfiguration('robot'), + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="kr_crazyflie_interface", + plugin="SO3CmdToCrazyflie", + name="so3cmd_to_crazyflie", + parameters=[config_file], + remappings=[ + ("~/odom", odom_topic), + ("~/so3_cmd", so3_cmd_topic) + ] + ) + ], + output='screen' + ) + + return LaunchDescription([ + robot_arg, + odom_arg, + so3_cmd_arg, + so3cmd_to_crazyflie_component + ]) diff --git a/interfaces/kr_crazyflie_interface/nodelet_plugin.xml b/interfaces/kr_crazyflie_interface/nodelet_plugin.xml deleted file mode 100644 index 6337b86d..00000000 --- a/interfaces/kr_crazyflie_interface/nodelet_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - This reformats an so3_command and publishes it on compatabile Crazyflie topics - - - diff --git a/interfaces/kr_crazyflie_interface/package.xml b/interfaces/kr_crazyflie_interface/package.xml index 413943ce..1f0faf39 100644 --- a/interfaces/kr_crazyflie_interface/package.xml +++ b/interfaces/kr_crazyflie_interface/package.xml @@ -1,24 +1,25 @@ - + + + kr_crazyflie_interface 1.0.0 kr_crazyflie_interface Justin Thomas Aaron Weinstein + Pranav Shah BSD - - catkin + ament_cmake - - roscpp + rclcpp nav_msgs geometry_msgs kr_mav_msgs - nodelet + rclcpp_components - + ament_cmake diff --git a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp new file mode 100644 index 00000000..2e585961 --- /dev/null +++ b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_component.cpp @@ -0,0 +1,232 @@ +// This nodelet is meant to subscribe to so3 commands and then convert them +// to geometry_msgs/Twist which is the type for cmd_vel, the crazyflie control +// topic. The format of this is as follows: +// linear.y = roll [-30 to 30 degrees] (may be negative) +// linear.x = pitch [-30 to 30 degrees] (may be negative) +// linear.z = thrust [0 to 60,000] (motors stiction around 2000) +// angular.z = yawrate [-200 to 200 degrees/second] (note this is not yaw!) + +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "kr_mav_msgs/msg/so3_command.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" + +// TODO: Remove CLAMP as macro +#define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x) + +class SO3CmdToCrazyflie : public rclcpp::Node +{ + public: + explicit SO3CmdToCrazyflie(const rclcpp::NodeOptions &options); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + void so3_cmd_callback(const kr_mav_msgs::msg::SO3Command::UniquePtr msg); + void odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom); + + bool odom_set_, so3_cmd_set_; + Eigen::Quaterniond q_odom_; + + rclcpp::Publisher::SharedPtr crazy_fast_cmd_vel_pub_, crazy_cmd_vel_pub_; + + rclcpp::Subscription::SharedPtr so3_cmd_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + + double so3_cmd_timeout_; + rclcpp::Time last_so3_cmd_time_; + kr_mav_msgs::msg::SO3Command last_so3_cmd_; + + double c1_; + double c2_; + double c3_; + + // TODO get rid of this for the gains coming in + double kp_yaw_rate_; + + int thrust_pwm_min_; // Necessary to overcome stiction + int thrust_pwm_max_; // Mapped to PWM + + int motor_status_; +}; + +void SO3CmdToCrazyflie::odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom) +{ + if(!odom_set_) + odom_set_ = true; + + q_odom_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, + odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); + + if(so3_cmd_set_ && ((this->now() - last_so3_cmd_time_).seconds() >= so3_cmd_timeout_)) + { + auto last_so3_cmd_ptr = std::make_unique(last_so3_cmd_); + + so3_cmd_callback(std::move(last_so3_cmd_ptr)); + } +} + +void SO3CmdToCrazyflie::so3_cmd_callback(kr_mav_msgs::msg::SO3Command::UniquePtr msg) +{ + if(!so3_cmd_set_) + so3_cmd_set_ = true; + + // switch on motors + if(msg->aux.enable_motors) + { + // If the crazyflie motors are timed out, we need to send a zero message in order to get them to start + if(motor_status_ < 3) + { + auto motors_vel_cmd = std::make_unique(); + crazy_cmd_vel_pub_->publish(std::move(motors_vel_cmd)); + last_so3_cmd_ = *msg; + last_so3_cmd_time_ = msg->header.stamp; + motor_status_ += 1; + return; + } + // After sending zero message send min thrust + if(motor_status_ < 10) + { + auto motors_vel_cmd = std::make_unique(); + motors_vel_cmd->linear.z = thrust_pwm_min_; + crazy_cmd_vel_pub_->publish(std::move(motors_vel_cmd)); + } + motor_status_ += 1; + } + else if(!msg->aux.enable_motors) + { + motor_status_ = 0; + auto motors_vel_cmd = std::make_unique(); + crazy_cmd_vel_pub_->publish(std::move(motors_vel_cmd)); + last_so3_cmd_ = *msg; + last_so3_cmd_time_ = msg->header.stamp; + return; + } + + // grab desired forces and rotation from so3 + const Eigen::Vector3d f_des(msg->force.x, msg->force.y, msg->force.z); + + const Eigen::Quaterniond q_des(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z); + + // check psi for stability + const Eigen::Matrix3d R_des(q_des); + const Eigen::Matrix3d R_cur(q_odom_); + + const float yaw_cur = std::atan2(R_cur(1, 0), R_cur(0, 0)); + const float yaw_des = std::atan2(R_des(1, 0), R_des(0, 0)); + + // Map these into the current body frame (based on yaw) + Eigen::Matrix3d R_des_new = R_des * Eigen::AngleAxisd(yaw_cur - yaw_des, Eigen::Vector3d::UnitZ()); + float pitch_des = -std::asin(R_des_new(2, 0)); + float roll_des = std::atan2(R_des_new(2, 1), R_des_new(2, 2)); + + roll_des = roll_des * 180 / M_PI; + pitch_des = pitch_des * 180 / M_PI; + + double thrust_f = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); // Force in Newtons + // RCLCPP_INFO(this->get_logger(), "thrust_f is %2.5f newtons", thrust_f); + thrust_f = std::max(thrust_f, 0.0); + + thrust_f = thrust_f * 1000 / 9.81; // Force in grams + // RCLCPP_INFO(this->get_logger(), "thrust_f is %2.5f grams", thrust_f); + + // RCLCPP_INFO(this->get_logger(), "coeffs are %2.2f, %2.2f, %2.2f", c1_, c2_, c3_); + float thrust_pwm = c1_ + c2_ * std::sqrt(c3_ + thrust_f); + + // RCLCPP_INFO(this->get_logger(), "thrust_pwm is %2.5f from 0-1", thrust_pwm); + + thrust_pwm = thrust_pwm * thrust_pwm_max_; // thrust_pwm mapped from 0-60000 + // RCLCPP_INFO(this->get_logger(), "thrust_pwm is calculated to be %2.5f in pwm", thrust_pwm); + + auto crazy_vel_cmd = std::make_unique(); + + float e_yaw = yaw_des - yaw_cur; + if(e_yaw > M_PI) + e_yaw -= 2 * M_PI; + else if(e_yaw < -M_PI) + e_yaw += 2 * M_PI; + + float yaw_rate_des = ((-msg->kr[2] * e_yaw) - msg->angular_velocity.z) * (180 / M_PI); + + // TODO change this check to be a param + // throttle the publish rate + // if ((this->now() - last_so3_cmd_time_).seconds() > 1.0/30.0){ + crazy_vel_cmd->linear.y = roll_des + msg->aux.angle_corrections[0]; + crazy_vel_cmd->linear.x = pitch_des + msg->aux.angle_corrections[1]; + crazy_vel_cmd->linear.z = CLAMP(thrust_pwm, thrust_pwm_min_, thrust_pwm_max_); + + // RCLCPP_INFO(this->get_logger(), "commanded thrust is %2.2f", CLAMP(thrust_pwm, thrust_pwm_min_, thrust_pwm_max_)); + + crazy_vel_cmd->angular.z = yaw_rate_des; + // RCLCPP_INFO(this->get_logger(), "commanded yaw rate is %2.2f", yaw_rate_des); //yaw_debug + + crazy_fast_cmd_vel_pub_->publish(std::move(crazy_vel_cmd)); + // save last so3_cmd + last_so3_cmd_ = *msg; + // last_so3_cmd_time_ = this->now(); + last_so3_cmd_time_ = msg->header.stamp; + //} + // else { + // RCLCPP_INFO_STREAM(this->get_logger(), "Commands too quick, time since is: " << (this->now() - + // last_so3_cmd_time_).seconds()); + //} +} + +SO3CmdToCrazyflie::SO3CmdToCrazyflie(const rclcpp::NodeOptions &options) + : Node("so3cmd_to_crazyflie", rclcpp::NodeOptions(options).use_intra_process_comms(true)) +{ + this->declare_parameter("kp_yaw_rate", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("c1", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("c2", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("c3", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("so3_cmd_timeout", 0.1); + this->declare_parameter("thrust_pwm_min", 10000); + this->declare_parameter("thrust_pwm_max", 60000); + + // get thrust scaling parameters + // Note that this is ignoring a constant based on the number of props, which + // is captured with the lin_cof_a variable later. + // + // TODO this is where we load thrust scaling stuff + if(this->get_parameter("kp_yaw_rate", kp_yaw_rate_)) + RCLCPP_INFO(this->get_logger(), "kp yaw rate is %2.2f", kp_yaw_rate_); + else + RCLCPP_FATAL(this->get_logger(), "kp yaw rate not found"); + + // get thrust scaling parameters + if(this->get_parameter("c1", c1_) && this->get_parameter("c2", c2_) && this->get_parameter("c3", c3_)) + RCLCPP_INFO(this->get_logger(), "Using %2.2f, %2.2f, %2.2f for thrust mapping", c1_, c2_, c3_); + else + RCLCPP_FATAL(this->get_logger(), "Must set coefficients for thrust scaling"); + + // get param for so3 command timeout duration + so3_cmd_timeout_ = this->get_parameter("so3_cmd_timeout").as_double(); + + thrust_pwm_max_ = this->get_parameter("thrust_pwm_max").as_int(); + thrust_pwm_min_ = this->get_parameter("thrust_pwm_min").as_int(); + + odom_set_ = false; + so3_cmd_set_ = false; + motor_status_ = 0; + + // TODO make sure this is publishing to the right place + crazy_fast_cmd_vel_pub_ = this->create_publisher("~/cmd_vel_fast", 10); + + crazy_cmd_vel_pub_ = this->create_publisher("~/cmd_vel", 10); + + // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos1 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + so3_cmd_sub_ = this->create_subscription( + "~/so3_cmd", qos2, std::bind(&SO3CmdToCrazyflie::so3_cmd_callback, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + "~/odom", qos1, std::bind(&SO3CmdToCrazyflie::odom_callback, this, std::placeholders::_1)); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(SO3CmdToCrazyflie) diff --git a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp b/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp deleted file mode 100644 index ddc8bb67..00000000 --- a/interfaces/kr_crazyflie_interface/src/so3cmd_to_crazyflie_nodelet.cpp +++ /dev/null @@ -1,231 +0,0 @@ -// This nodelet is meant to subscribe to so3 commands and then convert them -// to geometry_msgs/Twist which is the type for cmd_vel, the crazyflie control -// topic. The format of this is as follows: -// linear.y = roll [-30 to 30 degrees] (may be negative) -// linear.x = pitch [-30 to 30 degrees] (may be negative) -// linear.z = thrust [0 to 60,000] (motors stiction around 2000) -// angular.z = yawrate [-200 to 200 degrees/second] (note this is not yaw!) - -#include -#include -#include -#include -#include - -#include - -// TODO: Remove CLAMP as macro -#define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x) - -class SO3CmdToCrazyflie : public nodelet::Nodelet -{ - public: - void onInit(void); - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - private: - void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg); - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - - bool odom_set_, so3_cmd_set_; - Eigen::Quaterniond q_odom_; - - ros::Publisher attitude_raw_pub_; - ros::Publisher crazy_fast_cmd_vel_pub_, crazy_cmd_vel_pub_; - - ros::Subscriber so3_cmd_sub_; - ros::Subscriber odom_sub_; - - double so3_cmd_timeout_; - ros::Time last_so3_cmd_time_; - kr_mav_msgs::SO3Command last_so3_cmd_; - - double c1_; - double c2_; - double c3_; - - // TODO get rid of this for the gains coming in - double kp_yaw_rate_; - - int thrust_pwm_min_; // Necessary to overcome stiction - int thrust_pwm_max_; // Mapped to PWM - - int motor_status_; -}; - -void SO3CmdToCrazyflie::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) -{ - if(!odom_set_) - odom_set_ = true; - - q_odom_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, - odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); - - if(so3_cmd_set_ && ((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_)) - { - // ROS_WARN("so3_cmd timeout. %f seconds since last command", - // (ros::Time::now() - last_so3_cmd_time_).toSec()); - const auto last_so3_cmd_ptr = boost::make_shared(last_so3_cmd_); - - so3_cmd_callback(last_so3_cmd_ptr); - } -} - -void SO3CmdToCrazyflie::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg) -{ - if(!so3_cmd_set_) - so3_cmd_set_ = true; - - // switch on motors - if(msg->aux.enable_motors) - { - // If the crazyflie motors are timed out, we need to send a zero message in order to get them to start - if(motor_status_ < 3) - { - geometry_msgs::Twist::Ptr motors_vel_cmd(new geometry_msgs::Twist); - crazy_cmd_vel_pub_.publish(motors_vel_cmd); - last_so3_cmd_ = *msg; - last_so3_cmd_time_ = msg->header.stamp; - motor_status_ += 1; - return; - } - // After sending zero message send min thrust - if(motor_status_ < 10) - { - geometry_msgs::Twist::Ptr motors_vel_cmd(new geometry_msgs::Twist); - motors_vel_cmd->linear.z = thrust_pwm_min_; - crazy_cmd_vel_pub_.publish(motors_vel_cmd); - } - motor_status_ += 1; - } - else if(!msg->aux.enable_motors) - { - motor_status_ = 0; - geometry_msgs::Twist::Ptr motors_vel_cmd(new geometry_msgs::Twist); - crazy_cmd_vel_pub_.publish(motors_vel_cmd); - last_so3_cmd_ = *msg; - last_so3_cmd_time_ = msg->header.stamp; - return; - } - - /* - // If the crazyflie motors are timed out, we need to send a zero message in order to get them to start - if((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_){ - crazy_cmd_vel_pub_.publish(crazy_vel_cmd); - // Keep in mind that this publishes two messages in a row, this may cause bandwith problems - } - */ - - // grab desired forces and rotation from so3 - const Eigen::Vector3d f_des(msg->force.x, msg->force.y, msg->force.z); - - const Eigen::Quaterniond q_des(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z); - - // check psi for stability - const Eigen::Matrix3d R_des(q_des); - const Eigen::Matrix3d R_cur(q_odom_); - - const float yaw_cur = std::atan2(R_cur(1, 0), R_cur(0, 0)); - const float yaw_des = std::atan2(R_des(1, 0), R_des(0, 0)); - - // Map these into the current body frame (based on yaw) - Eigen::Matrix3d R_des_new = R_des * Eigen::AngleAxisd(yaw_cur - yaw_des, Eigen::Vector3d::UnitZ()); - float pitch_des = -std::asin(R_des_new(2, 0)); - float roll_des = std::atan2(R_des_new(2, 1), R_des_new(2, 2)); - - roll_des = roll_des * 180 / M_PI; - pitch_des = pitch_des * 180 / M_PI; - - double thrust_f = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); // Force in Newtons - // ROS_INFO("thrust_f is %2.5f newtons", thrust_f); - thrust_f = std::max(thrust_f, 0.0); - - thrust_f = thrust_f * 1000 / 9.81; // Force in grams - // ROS_INFO("thrust_f is %2.5f grams", thrust_f); - - // ROS_INFO("coeffs are %2.2f, %2.2f, %2.2f", c1_, c2_, c3_); - float thrust_pwm = c1_ + c2_ * std::sqrt(c3_ + thrust_f); - - // ROS_INFO("thrust_pwm is %2.5f from 0-1", thrust_pwm); - - thrust_pwm = thrust_pwm * thrust_pwm_max_; // thrust_pwm mapped from 0-60000 - // ROS_INFO("thrust_pwm is calculated to be %2.5f in pwm", thrust_pwm); - - geometry_msgs::Twist::Ptr crazy_vel_cmd(new geometry_msgs::Twist); - - float e_yaw = yaw_des - yaw_cur; - if(e_yaw > M_PI) - e_yaw -= 2 * M_PI; - else if(e_yaw < -M_PI) - e_yaw += 2 * M_PI; - - float yaw_rate_des = ((-msg->kR[2] * e_yaw) - msg->angular_velocity.z) * (180 / M_PI); - - // TODO change this check to be a param - // throttle the publish rate - // if ((ros::Time::now() - last_so3_cmd_time_).toSec() > 1.0/30.0){ - crazy_vel_cmd->linear.y = roll_des + msg->aux.angle_corrections[0]; - crazy_vel_cmd->linear.x = pitch_des + msg->aux.angle_corrections[1]; - crazy_vel_cmd->linear.z = CLAMP(thrust_pwm, thrust_pwm_min_, thrust_pwm_max_); - - // ROS_INFO("commanded thrust is %2.2f", CLAMP(thrust_pwm, thrust_pwm_min_, thrust_pwm_max_)); - - crazy_vel_cmd->angular.z = yaw_rate_des; - // ROS_INFO("commanded yaw rate is %2.2f", yaw_rate_des); //yaw_debug - - crazy_fast_cmd_vel_pub_.publish(crazy_vel_cmd); - // save last so3_cmd - last_so3_cmd_ = *msg; - // last_so3_cmd_time_ = ros::Time::now(); - last_so3_cmd_time_ = msg->header.stamp; - //} - // else { - // ROS_INFO_STREAM("Commands too quick, time since is: " << (ros::Time::now() - last_so3_cmd_time_).toSec()); - //} -} - -void SO3CmdToCrazyflie::onInit(void) -{ - ros::NodeHandle priv_nh(getPrivateNodeHandle()); - - // get thrust scaling parameters - // Note that this is ignoring a constant based on the number of props, which - // is captured with the lin_cof_a variable later. - // - // TODO this is where we load thrust scaling stuff - if(priv_nh.getParam("kp_yaw_rate", kp_yaw_rate_)) - ROS_INFO("kp yaw rate is %2.2f", kp_yaw_rate_); - else - ROS_FATAL("kp yaw rate not found"); - - // get thrust scaling parameters - if(priv_nh.getParam("c1", c1_) && priv_nh.getParam("c2", c2_) && priv_nh.getParam("c3", c3_)) - ROS_INFO("Using %2.2f, %2.2f, %2.2f for thrust mapping", c1_, c2_, c3_); - else - ROS_FATAL("Must set coefficients for thrust scaling"); - - // get param for so3 command timeout duration - priv_nh.param("so3_cmd_timeout", so3_cmd_timeout_, 0.1); - - priv_nh.param("thrust_pwm_max", thrust_pwm_max_, 60000); - priv_nh.param("thrust_pwm_min", thrust_pwm_min_, 10000); - - odom_set_ = false; - so3_cmd_set_ = false; - motor_status_ = 0; - - // TODO make sure this is publishing to the right place - crazy_fast_cmd_vel_pub_ = priv_nh.advertise("cmd_vel_fast", 10); - - crazy_cmd_vel_pub_ = priv_nh.advertise("cmd_vel", 10); - - so3_cmd_sub_ = - priv_nh.subscribe("so3_cmd", 1, &SO3CmdToCrazyflie::so3_cmd_callback, this, ros::TransportHints().tcpNoDelay()); - - odom_sub_ = - priv_nh.subscribe("odom", 10, &SO3CmdToCrazyflie::odom_callback, this, ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(SO3CmdToCrazyflie, nodelet::Nodelet); diff --git a/rqt_mav_manager/CMakeLists.txt b/rqt_mav_manager/CMakeLists.txt deleted file mode 100644 index 37663e2a..00000000 --- a/rqt_mav_manager/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(rqt_mav_manager) - -find_package(catkin REQUIRED) -catkin_python_setup() - -catkin_package() - -install(FILES plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -install(DIRECTORY resource DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -install(PROGRAMS scripts/rqt_mav_manager DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/rqt_mav_manager/package.xml b/rqt_mav_manager/package.xml index f0a65749..3de8618c 100644 --- a/rqt_mav_manager/package.xml +++ b/rqt_mav_manager/package.xml @@ -1,25 +1,32 @@ - + + + rqt_mav_manager - 0.1.0 + 0.2.0 Provides a GUI plugin for MAV Manager. Dinesh Thakur Kartik Mohta + pranav BSD - https://github.com/KumarRobotics/kr_ui + rclpy - Dinesh Thakur - - catkin - - rospy + ament_index_python + python3-pyside2 + python3-qt5-bindings + python_qt_binding + rqt_gui rqt_gui_py - kr_mav_manager + qt_gui_py_common std_srvs - rqt_gui + example_interfaces + kr_mav_manager + + python3-pytest + ament_python diff --git a/rqt_mav_manager/resource/rqt_mav_manager b/rqt_mav_manager/resource/rqt_mav_manager new file mode 100644 index 00000000..e69de29b diff --git a/rqt_mav_manager/rqt_mav_manager/__init__.py b/rqt_mav_manager/rqt_mav_manager/__init__.py new file mode 100644 index 00000000..cddef78d --- /dev/null +++ b/rqt_mav_manager/rqt_mav_manager/__init__.py @@ -0,0 +1,273 @@ +#!/usr/bin/env python3 + +from cairo import STATUS_INVALID_STATUS +import rclpy +import os + +from python_qt_binding import loadUi +from PyQt5.QtWidgets import QWidget +from rqt_gui_py.plugin import Plugin +from ament_index_python import get_resource + +import kr_mav_manager.srv +import std_srvs.srv +from example_interfaces.srv import AddTwoInts + +class MavManagerUi(Plugin): + + def __init__(self, context): + super().__init__(context) + self.setObjectName('MavManagerUi') + self._context = context + + self._context.node.declare_parameter('robot_name', "quadrotor") + + self.robot_name = self._context.node.get_parameter('robot_name').value + self.mav_node_name = 'mav_services' + + self._widget = QWidget() + _, package_path = get_resource("packages", "rqt_mav_manager") + ui_file = os.path.join(package_path, 'share', 'rqt_mav_manager', 'resource', 'MavManager.ui') + loadUi(ui_file, self._widget) + self._widget.setObjectName('MAVManagerWidget') + + if self._context.serial_number() > 1: + self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) + self._context.add_widget(self._widget) + + self._widget.robot_name_line_edit.textChanged.connect(self._on_robot_name_changed) + self._widget.node_name_line_edit.textChanged.connect(self._on_node_name_changed) + + self._widget.motors_on_push_button.pressed.connect(self._on_motors_on_pressed) + self._widget.motors_off_push_button.pressed.connect(self._on_motors_off_pressed) + self._widget.hover_push_button.pressed.connect(self._on_hover_pressed) + self._widget.ehover_push_button.pressed.connect(self._on_ehover_pressed) + self._widget.land_push_button.pressed.connect(self._on_land_pressed) + self._widget.eland_push_button.pressed.connect(self._on_eland_pressed) + self._widget.estop_push_button.pressed.connect(self._on_estop_pressed) + self._widget.goto_push_button.pressed.connect(self._on_goto_pressed) + + self._widget.takeoff_push_button.pressed.connect(self._on_takeoff_pressed) + self._widget.gohome_push_button.pressed.connect(self._on_gohome_pressed) + + def _on_robot_name_changed(self, robot_name): + self.robot_name = str(robot_name) + + def _on_node_name_changed(self, node_name): + self.mav_node_name = str(node_name) + + def _on_motors_on_pressed(self): + motors_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/motors' + client = self._context.node.create_client(std_srvs.srv.SetBool, motors_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {motors_topic} not available") + return + + request = std_srvs.srv.SetBool.Request() + request.data = True + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('Motors on: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_motors_off_pressed(self): + motors_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/motors' + client = self._context.node.create_client(std_srvs.srv.SetBool, motors_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {motors_topic} not available") + return + + request = std_srvs.srv.SetBool.Request() + request.data = False + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('Motors off: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_hover_pressed(self): + hover_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/hover' + client = self._context.node.create_client(std_srvs.srv.Trigger, hover_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {hover_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('Hover: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_ehover_pressed(self): + ehover_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/ehover' + client = self._context.node.create_client(std_srvs.srv.Trigger, ehover_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {ehover_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('EHover: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_land_pressed(self): + land_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/land' + client = self._context.node.create_client(std_srvs.srv.Trigger, land_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {land_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('Land: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_eland_pressed(self): + eland_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/eland' + client = self._context.node.create_client(std_srvs.srv.Trigger, eland_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {eland_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('ELand: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_estop_pressed(self): + estop_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/estop' + client = self._context.node.create_client(std_srvs.srv.Trigger, estop_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {estop_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('EStop: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_takeoff_pressed(self): + takeoff_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/takeoff' + client = self._context.node.create_client(std_srvs.srv.Trigger, takeoff_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {takeoff_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('Takeoff: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_gohome_pressed(self): + gohome_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/goHome' + client = self._context.node.create_client(std_srvs.srv.Trigger, gohome_topic) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {gohome_topic} not available") + return + + request = std_srvs.srv.Trigger.Request() + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('goHome: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + def _on_goto_pressed(self): + request = kr_mav_manager.srv.Vec4.Request() + request.goal[0] = self._widget.x_doubleSpinBox.value() + request.goal[1] = self._widget.y_doubleSpinBox.value() + request.goal[2] = self._widget.z_doubleSpinBox.value() + request.goal[3] = self._widget.yaw_doubleSpinBox.value() + + print(request.goal) + + if(self._widget.relative_checkbox.isChecked()): + goto_relative = '/'+self.robot_name+'/'+self.mav_node_name+'/goToRelative' + client = self._context.node.create_client(kr_mav_manager.srv.Vec4, goto_relative) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {goto_relative} not available") + return + + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('goToRelative: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + else: + goto = '/'+self.robot_name+'/'+self.mav_node_name+'/goTo' + client = self._context.node.create_client(kr_mav_manager.srv.Vec4, goto) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {goto} not available") + return + + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + response = future.result() + print('goTo: ', response.success) + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + + + # Qt Methods + def shutdown_plugin(self): + return super().shutdown_plugin() + + def save_settings(self, plugin_settings, instance_settings): + instance_settings.set_value('robot_name', self._widget.robot_name_line_edit.text()) + instance_settings.set_value('node_name' , self._widget.node_name_line_edit.text()) + + def restore_settings(self, plugin_settings, instance_settings): + + #Override saved value with param value if set + param_value = self._context.node.get_parameter('robot_name').value + self.robot_name = param_value + self._widget.robot_name_line_edit.setText(param_value) + + value = instance_settings.value('node_name', "mav_services") + self.node_name = value + self._widget.node_name_line_edit.setText(value) + diff --git a/rqt_mav_manager/rqt_mav_manager/rqt_mav_manager.py b/rqt_mav_manager/rqt_mav_manager/rqt_mav_manager.py new file mode 100644 index 00000000..7275338a --- /dev/null +++ b/rqt_mav_manager/rqt_mav_manager/rqt_mav_manager.py @@ -0,0 +1,13 @@ +#!/usr/bin/env python3 + +import sys + +from rqt_gui.main import Main + +def main(): +# Run the plugin + main = Main() + sys.exit(main.main(sys.argv, standalone="rqt_mav_manager.MavManagerUi")) + +if __name__ == "__main__": + main() diff --git a/rqt_mav_manager/scripts/rqt_mav_manager b/rqt_mav_manager/scripts/rqt_mav_manager deleted file mode 100755 index 44043283..00000000 --- a/rqt_mav_manager/scripts/rqt_mav_manager +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python3 - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_mav_manager.MavManagerUi')) diff --git a/rqt_mav_manager/setup.cfg b/rqt_mav_manager/setup.cfg new file mode 100644 index 00000000..9037326c --- /dev/null +++ b/rqt_mav_manager/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rqt_mav_manager +[install] +install_scripts=$base/lib/rqt_mav_manager diff --git a/rqt_mav_manager/setup.py b/rqt_mav_manager/setup.py index 6268a9c3..d58e062a 100644 --- a/rqt_mav_manager/setup.py +++ b/rqt_mav_manager/setup.py @@ -1,9 +1,28 @@ -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import find_packages, setup -d = generate_distutils_setup( - packages=['rqt_mav_manager'], - package_dir={'': 'src'}, -) +package_name = 'rqt_mav_manager' -setup(**d) +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name + '/resource', ['resource/MavManager.ui']), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, ['plugin.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Pranav Shah', + maintainer_email='pranavpshah2098@gmail.com', + description='TODO: Package description', + license='BSD', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + "rqt_mav_manager = rqt_mav_manager.rqt_mav_manager:main" + ], + }, +) diff --git a/rqt_mav_manager/src/rqt_mav_manager/__init__.py b/rqt_mav_manager/src/rqt_mav_manager/__init__.py deleted file mode 100644 index 85665c2a..00000000 --- a/rqt_mav_manager/src/rqt_mav_manager/__init__.py +++ /dev/null @@ -1,211 +0,0 @@ -from __future__ import print_function - -import os -import rospkg - -import rospy -from python_qt_binding import loadUi -from python_qt_binding.QtWidgets import QWidget -from rqt_gui_py.plugin import Plugin - -import kr_mav_manager.srv -import std_srvs.srv - -class MavManagerUi(Plugin): - - def __init__(self, context): - super(MavManagerUi, self).__init__(context) - self.setObjectName('MavManagerUi') - - self._publisher = None - - self.robot_name = 'quadrotor' - self.mav_node_name = 'mav_services' - - self._widget = QWidget() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_mav_manager'), 'resource', 'MavManager.ui') - loadUi(ui_file, self._widget) - self._widget.setObjectName('MavManagerWidget') - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - self._widget.robot_name_line_edit.textChanged.connect(self._on_robot_name_changed) - self._widget.node_name_line_edit.textChanged.connect(self._on_node_name_changed) - - self._widget.motors_on_push_button.pressed.connect(self._on_motors_on_pressed) - self._widget.motors_off_push_button.pressed.connect(self._on_motors_off_pressed) - self._widget.hover_push_button.pressed.connect(self._on_hover_pressed) - self._widget.ehover_push_button.pressed.connect(self._on_ehover_pressed) - self._widget.land_push_button.pressed.connect(self._on_land_pressed) - self._widget.eland_push_button.pressed.connect(self._on_eland_pressed) - self._widget.estop_push_button.pressed.connect(self._on_estop_pressed) - self._widget.goto_push_button.pressed.connect(self._on_goto_pressed) - - self._widget.takeoff_push_button.pressed.connect(self._on_takeoff_pressed) - self._widget.gohome_push_button.pressed.connect(self._on_gohome_pressed) - - def _on_robot_name_changed(self, robot_name): - self.robot_name = str(robot_name) - - def _on_node_name_changed(self, node_name): - self.mav_node_name = str(node_name) - - def _on_motors_on_pressed(self): - try: - motors_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/motors' - rospy.wait_for_service(motors_topic, timeout=1.0) - motors_on = rospy.ServiceProxy(motors_topic, std_srvs.srv.SetBool) - resp = motors_on(True) - print('Motors on ', resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_motors_off_pressed(self): - try: - motors_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/motors' - rospy.wait_for_service(motors_topic, timeout=1.0) - motors_off = rospy.ServiceProxy(motors_topic, std_srvs.srv.SetBool) - resp = motors_off(False) - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_hover_pressed(self): - try: - hover_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/hover' - rospy.wait_for_service(hover_topic, timeout=1.0) - hover = rospy.ServiceProxy(hover_topic, std_srvs.srv.Trigger) - resp = hover() - print('Hover ', resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_ehover_pressed(self): - try: - ehover_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/ehover' - rospy.wait_for_service(ehover_topic, timeout=1.0) - ehover = rospy.ServiceProxy(ehover_topic, std_srvs.srv.Trigger) - resp = ehover() - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_land_pressed(self): - try: - land_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/land' - rospy.wait_for_service(land_topic, timeout=1.0) - land = rospy.ServiceProxy(land_topic, std_srvs.srv.Trigger) - resp = land() - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_eland_pressed(self): - try: - eland_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/eland' - rospy.wait_for_service(eland_topic, timeout=1.0) - eland = rospy.ServiceProxy(eland_topic, std_srvs.srv.Trigger) - resp = eland() - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_estop_pressed(self): - try: - estop_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/estop' - rospy.wait_for_service(estop_topic, timeout=1.0) - estop = rospy.ServiceProxy(estop_topic, std_srvs.srv.Trigger) - resp = estop() - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_takeoff_pressed(self): - try: - takeoff_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/takeoff' - rospy.wait_for_service(takeoff_topic, timeout=1.0) - takeoff = rospy.ServiceProxy(takeoff_topic, std_srvs.srv.Trigger) - resp = takeoff() - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_gohome_pressed(self): - try: - gohome_topic = '/'+self.robot_name+'/'+self.mav_node_name+'/goHome' - rospy.wait_for_service(gohome_topic, timeout=1.0) - gohome = rospy.ServiceProxy(gohome_topic, std_srvs.srv.Trigger) - resp = gohome() - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - except(rospy.ROSException) as e: - print("Service call failed: %s"%e) - - def _on_goto_pressed(self): - - req = kr_mav_manager.srv.Vec4Request() - - req.goal[0] = self._widget.x_doubleSpinBox.value() - req.goal[1] = self._widget.y_doubleSpinBox.value() - req.goal[2] = self._widget.z_doubleSpinBox.value() - req.goal[3] = self._widget.yaw_doubleSpinBox.value() - - print(req.goal) - - if(self._widget.relative_checkbox.isChecked()): - try: - goto = rospy.ServiceProxy('/'+self.robot_name+'/'+self.mav_node_name+'/goToRelative', kr_mav_manager.srv.Vec4) - resp = goto(req) - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - else: - try: - goto_relative = rospy.ServiceProxy('/'+self.robot_name+'/'+self.mav_node_name+'/goTo', kr_mav_manager.srv.Vec4) - resp = goto_relative(req) - print(resp.success) - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - - def _unregister_publisher(self): - if self._publisher is not None: - self._publisher.unregister() - self._publisher = None - - def shutdown_plugin(self): - self._unregister_publisher() - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('robot_name' , self._widget.robot_name_line_edit.text()) - instance_settings.set_value('node_name' , self._widget.node_name_line_edit.text()) - - def restore_settings(self, plugin_settings, instance_settings): - - #Override saved value with param value if set - value = instance_settings.value('robot_name', "quadrotor") - param_value = rospy.get_param("robot_name", value) - self.robot_name = param_value - self._widget.robot_name_line_edit.setText(param_value) - - value = instance_settings.value('node_name', "mav_services") - self.node_name = value - self._widget.node_name_line_edit.setText(value)