From 91ceb7e19b114691768b6b78768fc4a023854b38 Mon Sep 17 00:00:00 2001 From: Unknown Date: Fri, 8 Feb 2019 11:53:36 +0100 Subject: [PATCH 1/4] starting . . . . . . . --- uav_abstraction_layer/CMakeLists.txt | 2 +- .../uav_abstraction_layer/backend_crazyflie.h | 227 +++++ uav_abstraction_layer/src/backend.cpp | 4 + .../src/backend_crazyflie.cpp | 854 ++++++++++++++++++ uav_abstraction_layer/src/ual.cpp | 1 + 5 files changed, 1087 insertions(+), 1 deletion(-) create mode 100644 uav_abstraction_layer/include/uav_abstraction_layer/backend_crazyflie.h create mode 100644 uav_abstraction_layer/src/backend_crazyflie.cpp diff --git a/uav_abstraction_layer/CMakeLists.txt b/uav_abstraction_layer/CMakeLists.txt index 116a940..3b7e761 100644 --- a/uav_abstraction_layer/CMakeLists.txt +++ b/uav_abstraction_layer/CMakeLists.txt @@ -150,7 +150,7 @@ include_directories( # src/${PROJECT_NAME}/uav_abstraction_layer.cpp # ) add_library(${PROJECT_NAME} - src/backend.cpp src/backend_mavros.cpp src/backend_light.cpp src/ual.cpp + src/backend.cpp src/backend_mavros.cpp src/backend_light.cpp src/backend_crazyflie src/ual.cpp ) ## Add cmake target dependencies of the library diff --git a/uav_abstraction_layer/include/uav_abstraction_layer/backend_crazyflie.h b/uav_abstraction_layer/include/uav_abstraction_layer/backend_crazyflie.h new file mode 100644 index 0000000..1e9360d --- /dev/null +++ b/uav_abstraction_layer/include/uav_abstraction_layer/backend_crazyflie.h @@ -0,0 +1,227 @@ +//---------------------------------------------------------------------------------------------------------------------- +// GRVC UAL +//---------------------------------------------------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2016 GRVC University of Seville +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to +// permit persons to whom the Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the +// Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS +// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +//---------------------------------------------------------------------------------------------------------------------- +#ifndef UAV_ABSTRACTION_LAYER_BACKEND_CRAZYFLIE_H +#define UAV_ABSTRACTION_LAYER_BACKEND_CRAZYFLIE_H + +#include +#include +#include + +#include +#include + +//Mavros services +#include +#include + +//Mavros messages +#include +#include +#include +#include +#include +#include +#include +#include + +// +#include +#include + +namespace grvc { namespace ual { + +// class HistoryBuffer { // TODO: template? utils? +// public: +// void set_size(size_t _size) { +// std::lock_guard lock(mutex_); +// buffer_size_ = _size; +// buffer_.clear(); +// current_ = 0; +// } + +// void reset() { +// std::lock_guard lock(mutex_); +// buffer_.clear(); +// current_ = 0; +// } + +// void update(double _value) { +// std::lock_guard lock(mutex_); +// if (buffer_.size() < buffer_size_) { +// buffer_.push_back(_value); +// } else { +// buffer_[current_] = _value; +// current_ = (current_ + 1) % buffer_size_; +// } +// } + +// bool get_stats(double& _min, double& _mean, double& _max) { +// std::lock_guard lock(mutex_); +// if (buffer_.size() >= buffer_size_) { +// double min_value = +std::numeric_limits::max(); +// double max_value = -std::numeric_limits::max(); +// double sum = 0; +// for (int i = 0; i < buffer_.size(); i++) { +// if (buffer_[i] < min_value) { min_value = buffer_[i]; } +// if (buffer_[i] > max_value) { max_value = buffer_[i]; } +// sum += buffer_[i]; +// } +// _min = min_value; +// _max = max_value; +// _mean = sum / buffer_.size(); +// return true; +// } +// return false; +// } + +// bool get_variance(double& _var) { +// std::lock_guard lock(mutex_); +// if (buffer_.size() >= buffer_size_) { +// double mean = 0; +// double sum = 0; +// _var = 0; +// for (int i = 0; i < buffer_.size(); i++) { +// sum += buffer_[i]; +// } +// mean = sum / buffer_.size(); +// for (int i = 0; i < buffer_.size(); i++) { +// _var += (buffer_[i]-mean)*(buffer_[i]-mean); +// } +// return true; +// } +// return false; +// } + +// protected: +// size_t buffer_size_ = 0; +// unsigned int current_ = 0; +// std::vector buffer_; +// std::mutex mutex_; +// }; + +class BackendCrazyflie : public Backend { + +public: + BackendCrazyflie(); + ~BackendCrazyflie(); + + /// Backend is initialized and ready to run tasks? + bool isReady() const override; + /// Latest pose estimation of the robot + virtual Pose pose() override; + /// Latest velocity estimation of the robot + virtual Velocity velocity() const override; + /// Latest odometry estimation of the robot + virtual Odometry odometry() const override; + /// Latest transform estimation of the robot + virtual Transform transform() const override; + + /// Set pose + /// \param _pose target pose + void setPose(const geometry_msgs::PoseStamped& _pose) override; + + /// Go to the specified waypoint, following a straight line + /// \param _wp goal waypoint + void goToWaypoint(const Waypoint& _wp) override; + + /// Go to the specified waypoint in geographic coordinates, following a straight line + /// \param _wp goal waypoint in geographic coordinates + void goToWaypointGeo(const WaypointGeo& _wp); + + /// Follow a list of waypoints, one after another + // void trackPath(const Path& _path) override; + /// Perform a take off maneuver + /// \param _height target height that must be reached to consider the take off complete + void takeOff(double _height) override; + /// Land on the current position. + void land() override; + /// Set velocities + /// \param _vel target velocity in world coordinates + void setVelocity(const Velocity& _vel) override; + /// Recover from manual flight mode + /// Use it when FLYING uav is switched to manual mode and want to go BACK to auto. + void recoverFromManual() override; + /// Set home position + void setHome(bool set_z) override; + +private: + void offboardThreadLoop(); + void initHomeFrame(); + bool referencePoseReached(); + void setFlightMode(const std::string& _flight_mode); + double updateParam(const std::string& _param_id); + State guessState(); + + //WaypointList path_; + geometry_msgs::PoseStamped ref_pose_; + sensor_msgs::NavSatFix ref_pose_global_; + geometry_msgs::PoseStamped cur_pose_; + sensor_msgs::NavSatFix cur_geo_pose_; + geometry_msgs::TwistStamped ref_vel_; + geometry_msgs::TwistStamped cur_vel_; + std_msgs::Int8 crazyflie_state_; + mavros_msgs::ExtendedState mavros_extended_state_; + + //Control + enum class eControlMode {LOCAL_VEL, LOCAL_POSE, GLOBAL_POSE}; + eControlMode control_mode_ = eControlMode::LOCAL_POSE; + bool mavros_has_pose_ = false; + bool mavros_has_geo_pose_ = false; + float position_th_; + float orientation_th_; + // HistoryBuffer position_error_; + // HistoryBuffer orientation_error_; + + /// Ros Communication + ros::ServiceClient flight_mode_client_; + ros::ServiceClient arming_client_; + ros::ServiceClient get_param_client_; + ros::ServiceClient takeoff_client_; + ros::ServiceClient land_client_; + ros::Publisher crazyflie_ref_pose_pub_; + ros::Publisher crazyflie_ref_pose_global_pub_; + ros::Publisher crazyflie_ref_vel_pub_; + ros::Subscriber crazyflie_cur_pose_sub_; + ros::Subscriber crazyflie_cur_geo_pose_sub_; + ros::Subscriber crazyflie_cur_vel_sub_; + ros::Subscriber crazyflie_cur_state_sub_; + ros::Subscriber crazyflie_cur_extended_state_sub_; + + int robot_id_; + std::string pose_frame_id_; + std::string uav_home_frame_id_; + std::string uav_frame_id_; + tf2_ros::StaticTransformBroadcaster * static_tf_broadcaster_; + std::map cached_transforms_; + std::map mavros_params_; + Eigen::Vector3d local_start_pos_; + ros::Time last_command_time_; + + std::thread offboard_thread_; + double offboard_thread_frequency_; + + bool calling_takeoff = false; + bool calling_land = false; +}; + +}} // namespace grvc::ual + +#endif // UAV_ABSTRACTION_LAYER_BACKEND_CRAZYFLIE_H diff --git a/uav_abstraction_layer/src/backend.cpp b/uav_abstraction_layer/src/backend.cpp index 5a8e17b..da94775 100644 --- a/uav_abstraction_layer/src/backend.cpp +++ b/uav_abstraction_layer/src/backend.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace grvc { namespace ual { @@ -48,6 +49,9 @@ Backend* Backend::createBackend() { else if (selected_backend == "dummy") { be = new BackendDummy(); } + else if (selected_backend == "crazyflie"){ + be = new BackendCrazyflie(); + } return be; } diff --git a/uav_abstraction_layer/src/backend_crazyflie.cpp b/uav_abstraction_layer/src/backend_crazyflie.cpp new file mode 100644 index 0000000..18ed15e --- /dev/null +++ b/uav_abstraction_layer/src/backend_crazyflie.cpp @@ -0,0 +1,854 @@ +//---------------------------------------------------------------------------------------------------------------------- +// GRVC UAL +//---------------------------------------------------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2016 GRVC University of Seville +// +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to +// permit persons to whom the Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the +// Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS +// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +//---------------------------------------------------------------------------------------------------------------------- + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* Crazyflie services and topics + hector@hector-GRVC ~/ual_ws> rostopic list + /crazyflie/anchors_markers + /crazyflie/battery + /crazyflie/cmd_vel + /crazyflie/crazyflie_position + /crazyflie/goal + /crazyflie/imu + /crazyflie/magnetic_field + /crazyflie/marker + /crazyflie/pose + /crazyflie/pressure + /crazyflie/ranging + /crazyflie/rssi + /crazyflie/temperature + /tf + + hector@hector-GRVC ~/ual_ws> rosservice list + /add_crazyflie + /crazyflie/land + /crazyflie/takeoff +*/ + +/* UAL services and topics + /uav_1/ual/odom + /uav_1/ual/pose + /uav_1/ual/set_pose + /uav_1/ual/set_velocity + /uav_1/ual/state + /uav_1/ual/velocity +*/ + +namespace grvc { +namespace ual { + +BackendCrazyflie::BackendCrazyflie() + : Backend() { + // Parse arguments + ros::NodeHandle pnh("~"); + pnh.param("uav_id", robot_id_, 1); + pnh.param("pose_frame_id", pose_frame_id_, ""); + // float position_th_param, orientation_th_param; + // pnh.param("position_th", position_th_param, 0.33); + // pnh.param("orientation_th", orientation_th_param, 0.65); + // position_th_ = position_th_param * position_th_param; + // orientation_th_ = 0.5 * (1 - cos(orientation_th_param)); + + ROS_INFO("BackendCrazyflie constructor with id %d", robot_id_); + // ROS_INFO("BackendCrazyflie: thresholds = %f %f", position_th_, orientation_th_); + + // Init ros communications + ros::NodeHandle nh; + std::string crazyflie_ns = "ual"; + // std::string set_mode_srv = crazyflie_ns + "/set_mode"; + // std::string arming_srv = crazyflie_ns + "/cmd/arming"; + // std::string get_param_srv = crazyflie_ns + "/param/get"; + std::string set_pose_topic = /* crazyflie_ns + */ "/crazyflie/goal"; + // std::string set_pose_global_topic = crazyflie_ns + "/setpoint_raw/global"; + std::string set_vel_topic = /* crazyflie_ns + */ "/crazyflie/cmd_vel"; + std::string pose_topic = /* crazyflie_ns + */ "/crazyflie/pose"; + // std::string geo_pose_topic = crazyflie_ns + "/global_position/global"; + // std::string vel_topic = crazyflie_ns + "/local_position/velocity"; + std::string state_topic = /* crazyflie_ns + */ "/crazyflie/state"; + // std::string extended_state_topic = crazyflie_ns + "/extended_state"; + + // flight_mode_client_ = nh.serviceClient(set_mode_srv.c_str()); + // arming_client_ = nh.serviceClient(arming_srv.c_str()); + takeoff_client_ = nh.serviceClient("/crazyflie/takeoff"); + land_client_ = nh.serviceClient("/crazyflie/land"); + + crazyflie_ref_pose_pub_ = nh.advertise(set_pose_topic.c_str(), 1); + // mavros_ref_pose_global_pub_ = nh.advertise(set_pose_global_topic.c_str(), 1); + crazyflie_ref_vel_pub_ = nh.advertise(set_vel_topic.c_str(), 1); + + crazyflie_cur_pose_sub_ = nh.subscribe(pose_topic.c_str(), 1, + [this](const geometry_msgs::PoseStamped::ConstPtr& _msg) { + this->cur_pose_ = *_msg; + this->mavros_has_pose_ = true; + }); + // crazyflie_cur_vel_sub_ = nh.subscribe(vel_topic.c_str(), 1, + // [this](const geometry_msgs::TwistStamped::ConstPtr& _msg) { + // this->cur_vel_ = *_msg; + // this->cur_vel_.header.frame_id = this->uav_home_frame_id_; + // }); + // mavros_cur_geo_pose_sub_ = nh.subscribe(geo_pose_topic.c_str(), 1, + // [this](const sensor_msgs::NavSatFix::ConstPtr& _msg) { + // this->cur_geo_pose_ = *_msg; + // if (!this->mavros_has_geo_pose_) { + // if (_msg->position_covariance[0] < 1.2 && _msg->position_covariance[0] > 0 && _msg->header.seq > 100) { + // this->mavros_has_geo_pose_ = true; + // // ROS_INFO("Has Geo Pose! %f",_msg->position_covariance[0]); + // } + // } + // }); + crazyflie_cur_state_sub_ = nh.subscribe(state_topic.c_str(), 1, + [this](const std_msgs::Int8::ConstPtr& _msg) { + this->crazyflie_state_ = *_msg; + }); + // mavros_cur_extended_state_sub_ = nh.subscribe(extended_state_topic.c_str(), 1, + // [this](const mavros_msgs::ExtendedState::ConstPtr& _msg) { + // this->mavros_extended_state_ = *_msg; + // }); + + // Wait until mavros is connected + // while (!crazyflie_state_.connected && ros::ok()) { + // std::this_thread::sleep_for(std::chrono::milliseconds(200)); + // } + ROS_INFO("wait_for_service /takeoff"); + takeoff_client_.waitForExistence(); + ROS_INFO("found /takeoff"); + + // TODO: Check this and solve frames issue + initHomeFrame(); + + // Thread publishing target pose at 10Hz for offboard mode + offboard_thread_ = std::thread(&BackendCrazyflie::offboardThreadLoop, this); + + // Client to get parameters from mavros and required default values + // get_param_client_ = nh.serviceClient(get_param_srv.c_str()); + // mavros_params_["MPC_XY_VEL_MAX"] = 2.0; // [m/s] Default value + // mavros_params_["MPC_Z_VEL_MAX_UP"] = 3.0; // [m/s] Default value + // mavros_params_["MPC_Z_VEL_MAX_DN"] = 1.0; // [m/s] Default value + // mavros_params_["MC_YAWRATE_MAX"] = 200.0; // [deg/s] Default value + // mavros_params_["MPC_TKO_SPEED"] = 1.5; // [m/s] Default value + // Updating here is non-sense as service seems to be slow in waking up + + ROS_INFO("BackendCrazyflie %d running!", robot_id_); +} + +BackendCrazyflie::~BackendCrazyflie() { + if (offboard_thread_.joinable()) { + offboard_thread_.join(); + } +} + +void BackendCrazyflie::offboardThreadLoop() { + // ros::param::param("~mavros_offboard_rate", offboard_thread_frequency_, 30.0); + // double hold_pose_time = 3.0; // [s] TODO param? + // int buffer_size = std::ceil(hold_pose_time * offboard_thread_frequency_); + // position_error_.set_size(buffer_size); + // orientation_error_.set_size(buffer_size); + offboard_thread_frequency_ = 30.0; + ros::Rate rate(offboard_thread_frequency_); + while (ros::ok() /* && crazyflie_state_.data == 1 */) { + switch (control_mode_) { + case eControlMode::LOCAL_VEL: + // crazyflie_ref_vel_pub_.publish(ref_vel_); + // ref_pose_ = cur_pose_; + // if (ros::Time::now().toSec() - last_command_time_.toSec() >= 0.5) { + // control_mode_ = eControlMode::LOCAL_POSE; + // } + break; + case eControlMode::LOCAL_POSE: + ref_pose_.header.stamp = ros::Time::now(); + crazyflie_ref_pose_pub_.publish(ref_pose_); + ref_vel_.twist.linear.x = 0; + ref_vel_.twist.linear.y = 0; + ref_vel_.twist.linear.z = 0; + ref_vel_.twist.angular.z = 0; + break; + // case eControlMode::GLOBAL_POSE: + // ref_vel_.twist.linear.x = 0; + // ref_vel_.twist.linear.y = 0; + // ref_vel_.twist.linear.z = 0; + // ref_vel_.twist.angular.z = 0; + // ref_pose_ = cur_pose_; + + // mavros_msgs::GlobalPositionTarget msg; + // msg.latitude = ref_pose_global_.latitude; + // msg.longitude = ref_pose_global_.longitude; + // msg.altitude = ref_pose_global_.altitude; + // msg.header.stamp = ros::Time::now(); + // msg.coordinate_frame = mavros_msgs::GlobalPositionTarget::FRAME_GLOBAL_REL_ALT; + // msg.type_mask = 4088; //((4095^1)^2)^4; + + // mavros_ref_pose_global_pub_.publish(msg); + // break; + } + // Error history update + // double dx = ref_pose_.pose.position.x - cur_pose_.pose.position.x; + // double dy = ref_pose_.pose.position.y - cur_pose_.pose.position.y; + // double dz = ref_pose_.pose.position.z - cur_pose_.pose.position.z; + // double positionD = dx * dx + dy * dy + dz * dz; // Equals distance^2 + + // double quatInnerProduct = ref_pose_.pose.orientation.x * cur_pose_.pose.orientation.x + + // ref_pose_.pose.orientation.y * cur_pose_.pose.orientation.y + + // ref_pose_.pose.orientation.z * cur_pose_.pose.orientation.z + + // ref_pose_.pose.orientation.w * cur_pose_.pose.orientation.w; + // double orientationD = 1.0 - quatInnerProduct * quatInnerProduct; // Equals (1-cos(rotation))/2 + + // position_error_.update(positionD); + // orientation_error_.update(orientationD); + + // State update + this->state_ = guessState(); + + rate.sleep(); + } +} + +Backend::State BackendCrazyflie::guessState() { + // Sequentially checks allow state deduction + if (!this->isReady()) { + return UNINITIALIZED; + } + // if (!this->crazyflie_state_.armed) { + // return LANDED_DISARMED; + // } + if (this->isReady() && crazyflie_state_.data == 0) { + return LANDED_ARMED; + } + if (this->calling_takeoff) { + return TAKING_OFF; + } + if (this->calling_land) { + return LANDING; + } + // if (this->crazyflie_state_.mode == "OFFBOARD") { + // return FLYING_AUTO; + // } + // return FLYING_MANUAL; + return FLYING_AUTO; +} + +void BackendCrazyflie::setFlightMode(const std::string& _flight_mode) { + // mavros_msgs::SetMode flight_mode_service; + // flight_mode_service.request.base_mode = 0; + // flight_mode_service.request.custom_mode = _flight_mode; + // // Set mode: unabortable? + // while (crazyflie_state_.mode != _flight_mode && ros::ok()) { + // if (!flight_mode_client_.call(flight_mode_service)) { + // ROS_ERROR("Error in set flight mode [%s] service calling!", _flight_mode.c_str()); + // } + // std::this_thread::sleep_for(std::chrono::milliseconds(300)); + // #ifdef MAVROS_VERSION_BELOW_0_20_0 + // ROS_INFO("Set flight mode [%s] response.success = %s", _flight_mode.c_str(), + // flight_mode_service.response.success ? "true" : "false"); + // #else + // ROS_INFO("Set flight mode [%s] response.success = %s", _flight_mode.c_str(), + // flight_mode_service.response.mode_sent ? "true" : "false"); + // #endif + // ROS_INFO("Trying to set [%s] mode; crazyflie_state_.mode = [%s]", _flight_mode.c_str(), crazyflie_state_.mode.c_str()); + // } +} + +void BackendCrazyflie::recoverFromManual() { + // if (!crazyflie_state_.armed || mavros_extended_state_.landed_state != + // mavros_msgs::ExtendedState::LANDED_STATE_IN_AIR) { + // ROS_WARN("Unable to recover from manual mode (not flying!)"); + // return; + // } + + // if (crazyflie_state_.mode != "POSCTL" && + // crazyflie_state_.mode != "ALTCTL" && + // crazyflie_state_.mode != "STABILIZED") { + // ROS_WARN("Unable to recover from manual mode (not in manual!)"); + // return; + // } + + // // Set mode to OFFBOARD and state to FLYING + // ref_pose_ = cur_pose_; + // control_mode_ = eControlMode::LOCAL_POSE; + // setFlightMode("OFFBOARD"); + // ROS_INFO("Recovered from manual mode!"); +} + +void BackendCrazyflie::setHome(bool set_z) { + // double z_offset = set_z ? cur_pose_.pose.position.z : 0.0; + // local_start_pos_ = -Eigen::Vector3d(cur_pose_.pose.position.x, + // cur_pose_.pose.position.y, z_offset); +} + +void BackendCrazyflie::takeOff(double _height) { + if (_height < 0.0) { + ROS_ERROR("Takeoff height must be positive!"); + return; + } + calling_takeoff = true; + + control_mode_ = eControlMode::LOCAL_POSE; // Take off control is performed in position (not velocity) + ref_pose_ = cur_pose_; + ref_pose_.pose.position.z = _height; + crazyflie_ref_pose_pub_.publish(ref_pose_); + std_srvs::Empty empty; + takeoff_client_.call(empty); + ROS_INFO("Taking off!"); + // float acc_max = 1.0; // TODO: From param? + // float vel_max = updateParam("MPC_TKO_SPEED"); + + // float a = sqrt(_height * acc_max); + // if (a < vel_max) { + // vel_max = a; + // } + // float t1 = vel_max / acc_max; + // float h1 = 0.5 * acc_max * t1 * t1; + // float t2 = t1 + (_height - 2.0 * h1) / vel_max; + // // float h2 = _height - h1; + // float t3 = t2 + t1; + + // float t = 0.0; + // float delta_t = 0.1; // [s] + // ros::Rate rate(1.0 / delta_t); + + // ref_pose_ = cur_pose_; + // float base_z = cur_pose_.pose.position.z; + // float delta_z = 0; + + // // setFlightMode("OFFBOARD"); + // while ((t < t3) && ros::ok()) { // Unabortable! + // if (t < t1) { + // delta_z = 0.5 * acc_max * t * t; + // } else if (t < t2) { + // delta_z = h1 + vel_max * (t - t1); + // } else { + // delta_z = _height - 0.5 * acc_max * (t3 - t) * (t3 - t); + // } + + // if (delta_z > _height) { + // ROS_WARN("Unexpected delta_z value [%f]", delta_z); + // } else { + // ref_pose_.pose.position.z = base_z + delta_z; + // } + + // rate.sleep(); + // t += delta_t; + // } + // ref_pose_.pose.position.z = base_z + _height; + + // Now wait (unabortable!) + // while (!referencePoseReached() && (this->crazyflie_state_.mode == "OFFBOARD") && ros::ok()) { + // std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // } + while (crazyflie_state_.data == 2 && ros::ok()) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ROS_INFO("Flying!"); + calling_takeoff = false; + + // Update state right now! + this->state_ = guessState(); +} + +void BackendCrazyflie::land() { + calling_land = true; + + control_mode_ = eControlMode::LOCAL_POSE; // Back to control in position (just in case) + // Set land mode + // setFlightMode("AUTO.LAND"); + ref_pose_ = cur_pose_; + ref_pose_.pose.position.z = 0; + // Landing is unabortable! + // while ((this->crazyflie_state_.mode == "AUTO.LAND") && ros::ok()) { + // std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // if (mavros_extended_state_.landed_state == mavros_msgs::ExtendedState::LANDED_STATE_ON_GROUND) { + // // setArmed(false); // Disabled for safety and symmetry + // ROS_INFO("Landed!"); + // break; // Out-of-while condition + // } + // } + std_srvs::Empty empty; + ROS_INFO("Landing..."); + land_client_.call(empty); + while (crazyflie_state_.data == 3 && ros::ok()) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ROS_INFO("Landed!"); + calling_land = false; + + // Update state right now! + this->state_ = guessState(); +} + +void BackendCrazyflie::setVelocity(const Velocity& _vel) { + // TODO: WARNING + // control_mode_ = eControlMode::LOCAL_VEL; // Velocity control! + + // tf2_ros::Buffer tfBuffer; + // tf2_ros::TransformListener tfListener(tfBuffer); + // geometry_msgs::Vector3Stamped vel_in, vel_out; + // vel_in.header = _vel.header; + // vel_in.vector = _vel.twist.linear; + // std::string vel_frame_id = tf2::getFrameId(vel_in); + + // if (vel_frame_id == "map" || vel_frame_id == "" || vel_frame_id == uav_home_frame_id_) { + // // No transform is needed + // ref_vel_ = _vel; + // } else { + // // We need to transform + // geometry_msgs::TransformStamped transform; + // bool tf_exists = true; + // try { + // transform = tfBuffer.lookupTransform(uav_home_frame_id_, vel_frame_id, ros::Time(0), ros::Duration(0.3)); + // } catch (tf2::TransformException& ex) { + // ROS_WARN("%s", ex.what()); + // tf_exists = false; + // ref_vel_ = _vel; + // } + + // if (tf_exists) { + // tf2::doTransform(vel_in, vel_out, transform); + // ref_vel_.header = vel_out.header; + // ref_vel_.twist.linear = vel_out.vector; + // ref_vel_.twist.angular = _vel.twist.angular; + // } + // } + // last_command_time_ = ros::Time::now(); +} + +bool BackendCrazyflie::isReady() const { + // if (ros::param::has("~map_origin_geo")) { + // return mavros_has_geo_pose_; + // } else { + // return mavros_has_pose_ && (fabs(this->cur_pose_.pose.position.y) > 1e-8); // Means the filter has converged! + // } + return true; +} + +void BackendCrazyflie::setPose(const geometry_msgs::PoseStamped& _world) { + control_mode_ = eControlMode::LOCAL_POSE; // Control in position + + // geometry_msgs::PoseStamped homogen_world_pos; + // tf2_ros::Buffer tfBuffer; + // tf2_ros::TransformListener tfListener(tfBuffer); + // std::string waypoint_frame_id = tf2::getFrameId(_world); + + // if (waypoint_frame_id == "" || waypoint_frame_id == uav_home_frame_id_) { + // // No transform is needed + // homogen_world_pos = _world; + // } else { + // // We need to transform + // geometry_msgs::TransformStamped transformToHomeFrame; + + // if (cached_transforms_.find(waypoint_frame_id) == cached_transforms_.end()) { + // // waypoint_frame_id not found in cached_transforms_ + // transformToHomeFrame = tfBuffer.lookupTransform(uav_home_frame_id_, waypoint_frame_id, ros::Time(0), ros::Duration(1.0)); + // cached_transforms_[waypoint_frame_id] = transformToHomeFrame; // Save transform in cache + // } else { + // // found in cache + // transformToHomeFrame = cached_transforms_[waypoint_frame_id]; + // } + + // tf2::doTransform(_world, homogen_world_pos, transformToHomeFrame); + // } + + // // std::cout << "Going to waypoint: " << homogen_world_pos.pose.position << std::endl; + + // // Do we still need local_start_pos_? + // homogen_world_pos.pose.position.x -= local_start_pos_[0]; + // homogen_world_pos.pose.position.y -= local_start_pos_[1]; + // homogen_world_pos.pose.position.z -= local_start_pos_[2]; + + // ref_pose_.pose = homogen_world_pos.pose; + ref_pose_ = _world; +} + +/* + // TODO: Move from here? + // struct PurePursuitOutput { + // geometry_msgs::Point next; + // float t_lookahead; + // }; + + // TODO: Move from here? + // PurePursuitOutput PurePursuit(geometry_msgs::Point _current, geometry_msgs::Point _initial, geometry_msgs::Point _final, float _lookahead) { + + // PurePursuitOutput out; + // out.next = _current; + // out.t_lookahead = 0; + // if (_lookahead <= 0) { + // ROS_ERROR("Lookahead must be non-zero positive!"); + // return out; + // } + + // Eigen::Vector3f x0 = Eigen::Vector3f(_current.x, _current.y, _current.z); + // Eigen::Vector3f x1 = Eigen::Vector3f(_initial.x, _initial.y, _initial.z); + // Eigen::Vector3f x2 = Eigen::Vector3f(_final.x, _final.y, _final.z); + // Eigen::Vector3f p = x0; + + // Eigen::Vector3f x_21 = x2 - x1; + // float d_21 = x_21.norm(); + // float t_min = - x_21.dot(x1-x0) / (d_21*d_21); + + // Eigen::Vector3f closest_point = x1 + t_min*(x2-x1); + // float distance = (closest_point - x0).norm(); + + // float t_lookahead = t_min; + // if (_lookahead > distance) { + // float a = sqrt(_lookahead*_lookahead - distance*distance); + // t_lookahead = t_min + a/d_21; + // } + + // if (t_lookahead <= 0.0) { + // p = x1; + // t_lookahead = 0.0; + // // ROS_INFO("p = x1"); + // } else if (t_lookahead >= 1.0) { + // p = x2; + // t_lookahead = 1.0; + // // ROS_INFO("p = x2"); + // } else { + // p = x1 + t_lookahead*(x2-x1); + // // ROS_INFO("L = %f; norm(x0-p) = %f", _lookahead, (x0-p).norm()); + // } + + // out.next.x = p(0); + // out.next.y = p(1); + // out.next.z = p(2); + // out.t_lookahead = t_lookahead; + // return out; + // } +*/ + +void BackendCrazyflie::goToWaypoint(const Waypoint& _world) { + // TODO: WARNING + // control_mode_ = eControlMode::LOCAL_POSE; // Control in position + + // geometry_msgs::PoseStamped homogen_world_pos; + // tf2_ros::Buffer tfBuffer; + // tf2_ros::TransformListener tfListener(tfBuffer); + // std::string waypoint_frame_id = tf2::getFrameId(_world); + + // if ( waypoint_frame_id == "" || waypoint_frame_id == uav_home_frame_id_ ) { + // // No transform is needed + // homogen_world_pos = _world; + // } + // else { + // // We need to transform + // geometry_msgs::TransformStamped transformToHomeFrame; + + // if ( cached_transforms_.find(waypoint_frame_id) == cached_transforms_.end() ) { + // // waypoint_frame_id not found in cached_transforms_ + // transformToHomeFrame = tfBuffer.lookupTransform(uav_home_frame_id_, waypoint_frame_id, ros::Time(0), ros::Duration(1.0)); + // cached_transforms_[waypoint_frame_id] = transformToHomeFrame; // Save transform in cache + // } else { + // // found in cache + // transformToHomeFrame = cached_transforms_[waypoint_frame_id]; + // } + + // tf2::doTransform(_world, homogen_world_pos, transformToHomeFrame); + + // } + + // // std::cout << "Going to waypoint: " << homogen_world_pos.pose.position << std::endl; + + // // Do we still need local_start_pos_? + // homogen_world_pos.pose.position.x -= local_start_pos_[0]; + // homogen_world_pos.pose.position.y -= local_start_pos_[1]; + // homogen_world_pos.pose.position.z -= local_start_pos_[2]; + + // // Smooth pose reference passing! + // geometry_msgs::Point final_position = homogen_world_pos.pose.position; + // geometry_msgs::Point initial_position = cur_pose_.pose.position; + // double ab_x = final_position.x - initial_position.x; + // double ab_y = final_position.y - initial_position.y; + // double ab_z = final_position.z - initial_position.z; + + // Eigen::Quaterniond final_orientation = Eigen::Quaterniond(homogen_world_pos.pose.orientation.w, + // homogen_world_pos.pose.orientation.x, homogen_world_pos.pose.orientation.y, homogen_world_pos.pose.orientation.z); + // Eigen::Quaterniond initial_orientation = Eigen::Quaterniond(cur_pose_.pose.orientation.w, + // cur_pose_.pose.orientation.x, cur_pose_.pose.orientation.y, cur_pose_.pose.orientation.z); + + // float linear_distance = sqrt(ab_x*ab_x + ab_y*ab_y + ab_z*ab_z); + // float linear_threshold = sqrt(position_th_); + // if (linear_distance > linear_threshold) { + // float mpc_xy_vel_max = updateParam("MPC_XY_VEL_MAX"); + // float mpc_z_vel_max_up = updateParam("MPC_Z_VEL_MAX_UP"); + // float mpc_z_vel_max_dn = updateParam("MPC_Z_VEL_MAX_DN"); + // float mc_yawrate_max = updateParam("MC_YAWRATE_MAX"); + + // float mpc_z_vel_max = (ab_z > 0)? mpc_z_vel_max_up : mpc_z_vel_max_dn; + // float xy_distance = sqrt(ab_x*ab_x + ab_y*ab_y); + // float z_distance = fabs(ab_z); + // bool z_vel_is_limit = (mpc_z_vel_max*xy_distance < mpc_xy_vel_max*z_distance); + + // ros::Rate rate(10); // [Hz] + // float next_to_final_distance = linear_distance; + // float lookahead = 0.05; + // while (next_to_final_distance > linear_threshold && !abort_ && ros::ok()) { + // float current_xy_vel = sqrt(cur_vel_.twist.linear.x*cur_vel_.twist.linear.x + cur_vel_.twist.linear.y*cur_vel_.twist.linear.y); + // float current_z_vel = fabs(cur_vel_.twist.linear.z); + // if (z_vel_is_limit) { + // if (current_z_vel > 0.8*mpc_z_vel_max) { lookahead -= 0.05; } // TODO: Other thesholds, other update politics? + // if (current_z_vel < 0.5*mpc_z_vel_max) { lookahead += 0.05; } // TODO: Other thesholds, other update politics? + // // ROS_INFO("current_z_vel = %f", current_z_vel); + // } else { + // if (current_xy_vel > 0.8*mpc_xy_vel_max) { lookahead -= 0.05; } // TODO: Other thesholds, other update politics? + // if (current_xy_vel < 0.5*mpc_xy_vel_max) { lookahead += 0.05; } // TODO: Other thesholds, other update politics? + // // ROS_INFO("current_xy_vel = %f", current_xy_vel); + // } + // // PurePursuitOutput pp = PurePursuit(cur_pose_.pose.position, initial_position, final_position, lookahead); + // Waypoint wp_i; + // wp_i.pose.position.x = pp.next.x; + // wp_i.pose.position.y = pp.next.y; + // wp_i.pose.position.z = pp.next.z; + // Eigen::Quaterniond q_i = initial_orientation.slerp(pp.t_lookahead, final_orientation); + // wp_i.pose.orientation.w = q_i.w(); + // wp_i.pose.orientation.x = q_i.x(); + // wp_i.pose.orientation.y = q_i.y(); + // wp_i.pose.orientation.z = q_i.z(); + // ref_pose_.pose = wp_i.pose; + // next_to_final_distance = (1.0 - pp.t_lookahead) * linear_distance; + // // ROS_INFO("next_to_final_distance = %f", next_to_final_distance); + // rate.sleep(); + // } + // } + // // ROS_INFO("All points sent!"); + + // // Finally set pose + // ref_pose_.pose = homogen_world_pos.pose; + // // position_error_.reset(); + // // orientation_error_.reset(); + + // // Wait until we arrive: abortable + // while(!referencePoseReached() && !abort_ && ros::ok()) { + // std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // } + // // Freeze in case it's been aborted + // if (abort_ && freeze_) { + // ref_pose_ = cur_pose_; + // } +} + +void BackendCrazyflie::goToWaypointGeo(const WaypointGeo& _wp) { + // TODO: WARNING + // control_mode_ = eControlMode::GLOBAL_POSE; // Control in position + + // ref_pose_global_.latitude = _wp.latitude; + // ref_pose_global_.longitude = _wp.longitude; + // ref_pose_global_.altitude = _wp.altitude; + + // // Wait until we arrive: abortable + // while (!referencePoseReached() && !abort_ && ros::ok()) { + // std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // } + // // Freeze in case it's been aborted + // if (abort_ && freeze_) { + // ref_pose_ = cur_pose_; + // } +} + +/*void BackendCrazyflie::trackPath(const WaypointList &_path) { + // TODO: basic imlementation, ideally different from a stack of gotos +}*/ + +Pose BackendCrazyflie::pose() { + Pose out; + + out.pose.position.x = cur_pose_.pose.position.x /* + local_start_pos_[0] */; + out.pose.position.y = cur_pose_.pose.position.y /* + local_start_pos_[1] */; + out.pose.position.z = cur_pose_.pose.position.z /* + local_start_pos_[2] */; + out.pose.orientation = cur_pose_.pose.orientation; + + // if (pose_frame_id_ == "") { + // // Default: local pose + // out.header.frame_id = uav_home_frame_id_; + // } else { + // // Publish pose in different frame + // Pose aux = out; + // geometry_msgs::TransformStamped transformToPoseFrame; + // std::string pose_frame_id_map = "inv_" + pose_frame_id_; + + // if (cached_transforms_.find(pose_frame_id_map) == cached_transforms_.end()) { + // // inv_pose_frame_id_ not found in cached_transforms_ + // tf2_ros::Buffer tfBuffer; + // tf2_ros::TransformListener tfListener(tfBuffer); + // transformToPoseFrame = tfBuffer.lookupTransform(pose_frame_id_, uav_home_frame_id_, ros::Time(0), ros::Duration(1.0)); + // cached_transforms_[pose_frame_id_map] = transformToPoseFrame; // Save transform in cache + // } else { + // // found in cache + // transformToPoseFrame = cached_transforms_[pose_frame_id_map]; + // } + + // tf2::doTransform(aux, out, transformToPoseFrame); + // out.header.frame_id = pose_frame_id_; + // } + out.header.frame_id = "world"; + out.header.stamp = cur_pose_.header.stamp; + return out; +} + +Velocity BackendCrazyflie::velocity() const { + // TODO: WARNING + return cur_vel_; +} + +Odometry BackendCrazyflie::odometry() const { + // TODO: WARNING + Odometry odom; + + odom.header.stamp = ros::Time::now(); + odom.header.frame_id = uav_home_frame_id_; + odom.child_frame_id = uav_frame_id_; + odom.pose.pose.position.x = cur_pose_.pose.position.x /* + local_start_pos_[0] */; + odom.pose.pose.position.y = cur_pose_.pose.position.y /* + local_start_pos_[1] */; + odom.pose.pose.position.z = cur_pose_.pose.position.z /* + local_start_pos_[2] */; + odom.pose.pose.orientation = cur_pose_.pose.orientation; + odom.twist.twist = cur_vel_.twist; + + return odom; +} + +Transform BackendCrazyflie::transform() const { + // TODO: WARNING + Transform out; + out.header.stamp = ros::Time::now(); + out.header.frame_id = uav_home_frame_id_; + out.child_frame_id = uav_frame_id_; + out.transform.translation.x = cur_pose_.pose.position.x /* + local_start_pos_[0] */; + out.transform.translation.y = cur_pose_.pose.position.y /* + local_start_pos_[1] */; + out.transform.translation.z = cur_pose_.pose.position.z /* + local_start_pos_[2] */; + if (cur_pose_.pose.orientation.w == 0) { + // out.transform.rotation = cur_pose_.pose.orientation; + out.transform.rotation.w = cur_pose_.pose.orientation.w + 1; + } else { + out.transform.rotation = cur_pose_.pose.orientation; + } + return out; +} + +bool BackendCrazyflie::referencePoseReached() { + // double position_min, position_mean, position_max; + // double orientation_min, orientation_mean, orientation_max; + // // if (!position_error_.get_stats(position_min, position_mean, position_max)) { return false; } + // // if (!orientation_error_.get_stats(orientation_min, orientation_mean, orientation_max)) { return false; } + + // double position_diff = position_max - position_min; + // double orientation_diff = orientation_max - orientation_min; + // bool position_holds = (position_diff < position_th_) && (fabs(position_mean) < 0.5 * position_th_); + // bool orientation_holds = (orientation_diff < orientation_th_) && (fabs(orientation_mean) < 0.5 * orientation_th_); + + // // if (position_holds && orientation_holds) { // DEBUG + // // ROS_INFO("position: %f < %f) && (%f < %f)", position_diff, position_th_, fabs(position_mean), 0.5*position_th_); + // // ROS_INFO("orientation: %f < %f) && (%f < %f)", orientation_diff, orientation_th_, fabs(orientation_mean), 0.5*orientation_th_); + // // ROS_INFO("Arrived!"); + // // } + + // return position_holds && orientation_holds; +} + +void BackendCrazyflie::initHomeFrame() { + // local_start_pos_ << 0.0, 0.0, 0.0; + + // // Get frames from rosparam + ros::param::param("~uav_frame", uav_frame_id_, "uav_" + std::to_string(robot_id_)); + ros::param::param("~uav_home_frame", uav_home_frame_id_, "uav_" + std::to_string(robot_id_) + "_home"); + std::string parent_frame; + ros::param::param("~home_pose_parent_frame", parent_frame, "map"); + + // std::vector home_pose(3, 0.0); + // if (ros::param::has("~home_pose")) { + // ros::param::get("~home_pose", home_pose); + // } else if (ros::param::has("~map_origin_geo")) { + // ROS_WARN("Be careful, you should only use this mode with RTK GPS!"); + // while (!this->mavros_has_geo_pose_) { + // std::this_thread::sleep_for(std::chrono::milliseconds(200)); + // } + // std::vector map_origin_geo(3, 0.0); + // ros::param::get("~map_origin_geo", map_origin_geo); + // geographic_msgs::GeoPoint origin_geo, actual_coordinate_geo; + // origin_geo.latitude = map_origin_geo[0]; + // origin_geo.longitude = map_origin_geo[1]; + // origin_geo.altitude = 0; //map_origin_geo[2]; + // actual_coordinate_geo.latitude = cur_geo_pose_.latitude; + // actual_coordinate_geo.longitude = cur_geo_pose_.longitude; + // actual_coordinate_geo.altitude = 0; //cur_geo_pose_.altitude; + // if (map_origin_geo[0] == 0 && map_origin_geo[1] == 0) { + // ROS_WARN("Map origin is set to 0. Define map_origin_geo param by a vector in format [lat,lon,alt]."); + // } + // geometry_msgs::Point32 map_origin_cartesian = geographic_to_cartesian(actual_coordinate_geo, origin_geo); + + // home_pose[0] = map_origin_cartesian.x; + // home_pose[1] = map_origin_cartesian.y; + // home_pose[2] = map_origin_cartesian.z; + // } else { + // ROS_WARN("No home pose or map origin was defined. Home frame will be equal to map."); + // } + + // geometry_msgs::TransformStamped static_transformStamped; + + // static_transformStamped.header.stamp = ros::Time::now(); + // static_transformStamped.header.frame_id = parent_frame; + // static_transformStamped.child_frame_id = uav_home_frame_id_; + // static_transformStamped.transform.translation.x = home_pose[0]; + // static_transformStamped.transform.translation.y = home_pose[1]; + // static_transformStamped.transform.translation.z = home_pose[2]; + + // if (parent_frame == "map" || parent_frame == "") { + // static_transformStamped.transform.rotation.x = 0; + // static_transformStamped.transform.rotation.y = 0; + // static_transformStamped.transform.rotation.z = 0; + // static_transformStamped.transform.rotation.w = 1; + // } else { + // tf2_ros::Buffer tfBuffer; + // tf2_ros::TransformListener tfListener(tfBuffer); + // geometry_msgs::TransformStamped transform_to_map; + // transform_to_map = tfBuffer.lookupTransform(parent_frame, "map", ros::Time(0), ros::Duration(2.0)); + // static_transformStamped.transform.rotation = transform_to_map.transform.rotation; + // } + + // static_tf_broadcaster_ = new tf2_ros::StaticTransformBroadcaster(); + // static_tf_broadcaster_->sendTransform(static_transformStamped); +} + +double BackendCrazyflie::updateParam(const std::string& _param_id) { + // mavros_msgs::ParamGet get_param_service; + // get_param_service.request.param_id = _param_id; + // if (get_param_client_.call(get_param_service) && get_param_service.response.success) { + // mavros_params_[_param_id] = get_param_service.response.value.integer ? get_param_service.response.value.integer : get_param_service.response.value.real; + // ROS_INFO("Parameter [%s] value is [%f]", get_param_service.request.param_id.c_str(), mavros_params_[_param_id]); + // } else if (mavros_params_.count(_param_id)) { + // ROS_ERROR("Error in get param [%s] service calling, leaving current value [%f]", + // get_param_service.request.param_id.c_str(), mavros_params_[_param_id]); + // } else { + // mavros_params_[_param_id] = 0.0; + // ROS_ERROR("Error in get param [%s] service calling, initializing it to zero", + // get_param_service.request.param_id.c_str()); + // } + // return mavros_params_[_param_id]; +} +} // namespace ual +} // namespace grvc diff --git a/uav_abstraction_layer/src/ual.cpp b/uav_abstraction_layer/src/ual.cpp index 73fb936..7f32494 100644 --- a/uav_abstraction_layer/src/ual.cpp +++ b/uav_abstraction_layer/src/ual.cpp @@ -262,6 +262,7 @@ bool UAL::takeOff(double _height, bool _blocking) { // Check required state if (backend_->state() != Backend::State::LANDED_ARMED) { ROS_ERROR("Unable to takeOff: not LANDED_ARMED!"); + std::cout << backend_->state() << std::endl; return false; } // Check input From bcfb7abfc04e8c3a398160d507e4128391c1a5e7 Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 20 Feb 2019 08:37:17 +0100 Subject: [PATCH 2/4] Clean comments --- .../src/backend_crazyflie.cpp | 34 +------------------ 1 file changed, 1 insertion(+), 33 deletions(-) diff --git a/uav_abstraction_layer/src/backend_crazyflie.cpp b/uav_abstraction_layer/src/backend_crazyflie.cpp index 18ed15e..910fa4e 100644 --- a/uav_abstraction_layer/src/backend_crazyflie.cpp +++ b/uav_abstraction_layer/src/backend_crazyflie.cpp @@ -31,38 +31,6 @@ #include #include -/* Crazyflie services and topics - hector@hector-GRVC ~/ual_ws> rostopic list - /crazyflie/anchors_markers - /crazyflie/battery - /crazyflie/cmd_vel - /crazyflie/crazyflie_position - /crazyflie/goal - /crazyflie/imu - /crazyflie/magnetic_field - /crazyflie/marker - /crazyflie/pose - /crazyflie/pressure - /crazyflie/ranging - /crazyflie/rssi - /crazyflie/temperature - /tf - - hector@hector-GRVC ~/ual_ws> rosservice list - /add_crazyflie - /crazyflie/land - /crazyflie/takeoff -*/ - -/* UAL services and topics - /uav_1/ual/odom - /uav_1/ual/pose - /uav_1/ual/set_pose - /uav_1/ual/set_velocity - /uav_1/ual/state - /uav_1/ual/velocity -*/ - namespace grvc { namespace ual { @@ -241,7 +209,7 @@ Backend::State BackendCrazyflie::guessState() { // } if (this->isReady() && crazyflie_state_.data == 0) { return LANDED_ARMED; - } + } if (this->calling_takeoff) { return TAKING_OFF; } From 5386882e2675b8cb706805ffd85f28cbebd8a2e0 Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 20 Feb 2019 12:25:38 +0100 Subject: [PATCH 3/4] step 1 cmd_vel --- .../src/backend_crazyflie.cpp | 25 +++++++++++++------ 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/uav_abstraction_layer/src/backend_crazyflie.cpp b/uav_abstraction_layer/src/backend_crazyflie.cpp index 910fa4e..d1f149e 100644 --- a/uav_abstraction_layer/src/backend_crazyflie.cpp +++ b/uav_abstraction_layer/src/backend_crazyflie.cpp @@ -145,11 +145,11 @@ void BackendCrazyflie::offboardThreadLoop() { while (ros::ok() /* && crazyflie_state_.data == 1 */) { switch (control_mode_) { case eControlMode::LOCAL_VEL: - // crazyflie_ref_vel_pub_.publish(ref_vel_); - // ref_pose_ = cur_pose_; - // if (ros::Time::now().toSec() - last_command_time_.toSec() >= 0.5) { - // control_mode_ = eControlMode::LOCAL_POSE; - // } + crazyflie_ref_vel_pub_.publish(ref_vel_); + ref_pose_ = cur_pose_; + if (ros::Time::now().toSec() - last_command_time_.toSec() >= 0.5) { + control_mode_ = eControlMode::LOCAL_POSE; + } break; case eControlMode::LOCAL_POSE: ref_pose_.header.stamp = ros::Time::now(); @@ -373,7 +373,7 @@ void BackendCrazyflie::land() { void BackendCrazyflie::setVelocity(const Velocity& _vel) { // TODO: WARNING - // control_mode_ = eControlMode::LOCAL_VEL; // Velocity control! + control_mode_ = eControlMode::LOCAL_VEL; // Velocity control! // tf2_ros::Buffer tfBuffer; // tf2_ros::TransformListener tfListener(tfBuffer); @@ -404,7 +404,18 @@ void BackendCrazyflie::setVelocity(const Velocity& _vel) { // ref_vel_.twist.angular = _vel.twist.angular; // } // } - // last_command_time_ = ros::Time::now(); + ref_vel_.header = _vel.header; + ref_vel_.twist = _vel.twist; + // Do not change your Z ? + ref_vel_.twist.linear.z = cur_pose_.pose.position.z * 1000; + // Warning! LPS Python commands Z velocity like that + // if (_vel.twist.linear.z > 0) { + // ref_vel_.twist.linear.z = _vel.twist.linear.z * 1000; + // } else { + // ref_vel_.twist.linear.z = 1; + // } + + last_command_time_ = ros::Time::now(); } bool BackendCrazyflie::isReady() const { From d5f7b2c278fa5b3e5910861c7674ce5b3846bb12 Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 20 Feb 2019 14:12:11 +0100 Subject: [PATCH 4/4] robot_id_ --- uav_abstraction_layer/src/backend_crazyflie.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/uav_abstraction_layer/src/backend_crazyflie.cpp b/uav_abstraction_layer/src/backend_crazyflie.cpp index d1f149e..1768210 100644 --- a/uav_abstraction_layer/src/backend_crazyflie.cpp +++ b/uav_abstraction_layer/src/backend_crazyflie.cpp @@ -51,23 +51,23 @@ BackendCrazyflie::BackendCrazyflie() // Init ros communications ros::NodeHandle nh; - std::string crazyflie_ns = "ual"; + std::string crazyflie_ns = "/crazyflie" + robot_id_; // std::string set_mode_srv = crazyflie_ns + "/set_mode"; // std::string arming_srv = crazyflie_ns + "/cmd/arming"; // std::string get_param_srv = crazyflie_ns + "/param/get"; - std::string set_pose_topic = /* crazyflie_ns + */ "/crazyflie/goal"; + std::string set_pose_topic = crazyflie_ns + "/goal"; // std::string set_pose_global_topic = crazyflie_ns + "/setpoint_raw/global"; - std::string set_vel_topic = /* crazyflie_ns + */ "/crazyflie/cmd_vel"; - std::string pose_topic = /* crazyflie_ns + */ "/crazyflie/pose"; + std::string set_vel_topic = crazyflie_ns + "/cmd_vel"; + std::string pose_topic = crazyflie_ns + "/pose"; // std::string geo_pose_topic = crazyflie_ns + "/global_position/global"; // std::string vel_topic = crazyflie_ns + "/local_position/velocity"; - std::string state_topic = /* crazyflie_ns + */ "/crazyflie/state"; + std::string state_topic = crazyflie_ns + "/state"; // std::string extended_state_topic = crazyflie_ns + "/extended_state"; // flight_mode_client_ = nh.serviceClient(set_mode_srv.c_str()); // arming_client_ = nh.serviceClient(arming_srv.c_str()); - takeoff_client_ = nh.serviceClient("/crazyflie/takeoff"); - land_client_ = nh.serviceClient("/crazyflie/land"); + takeoff_client_ = nh.serviceClient(crazyflie_ns + "/takeoff"); + land_client_ = nh.serviceClient(crazyflie_ns + "/land"); crazyflie_ref_pose_pub_ = nh.advertise(set_pose_topic.c_str(), 1); // mavros_ref_pose_global_pub_ = nh.advertise(set_pose_global_topic.c_str(), 1);