//////////////////////////////////////////////////////////////////////////////// // Update the controller void GazeboQuadrotorAerodynamics::Update() { // Get simulator time Time current_time = world->GetSimTime(); Time dt = current_time - last_time_; last_time_ = current_time; if (dt <= 0.0) return; // Get new commands/state callback_queue_.callAvailable(); // fill input vector u for drag model geometry_msgs::Quaternion orientation; fromQuaternion(link->GetWorldPose().rot, orientation); model_.setOrientation(orientation); geometry_msgs::Twist twist; fromVector(link->GetWorldLinearVel(), twist.linear); fromVector(link->GetWorldAngularVel(), twist.angular); model_.setTwist(twist); // update the model model_.update(dt.Double()); // get wrench from model Vector3 force, torque; toVector(model_.getWrench().force, force); toVector(model_.getWrench().torque, torque); // set force and torque in gazebo link->AddRelativeForce(force); link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force)); }
#include "Catch/single_include/catch.hpp" #include "ListNode.hpp" TEST_CASE("ListNode") { std::vector<int> vec1 = {1, 2, 3, 4, 5}; std::vector<int> vec2 = vec1; std::vector<int> vec3 = {1, 2, 3, 4, 5, 6}; ListNode *l1 = fromVector(vec1); ListNode *l2 = fromVector(vec2); ListNode *l3 = fromVector(vec3); std::string output{"HEAD -> 1 -> 2 -> 3 -> 4 -> 5 -> END\n"}; std::stringstream out; std::string sout; REQUIRE(compareList(l1, l2) == true); REQUIRE(compareList(l1, l3) == false); //printList(out, l1); //std::cout << out.rdbuf(); }
//////////////////////////////////////////////////////////////////////////////// // Update the controller void GazeboQuadrotorPropulsion::Update() { // Get simulator time Time current_time = world->GetSimTime(); Time dt = current_time - last_time_; last_time_ = current_time; if (dt <= 0.0) return; // Send trigger bool trigger = controlTimer.getUpdatePeriod() > 0.0 ? controlTimer.update() : false; if (trigger && trigger_publisher_) { rosgraph_msgs::Clock clock; clock.clock = ros::Time(current_time.sec, current_time.nsec); trigger_publisher_.publish(clock); ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Sent a trigger message at t = " << current_time.Double() << " (dt = " << (current_time - last_trigger_time_).Double() << ")"); last_trigger_time_ = current_time; } // Get new commands/state callback_queue_.callAvailable(); // Process input queue model_.processQueue(ros::Time(current_time.sec, current_time.nsec), control_tolerance_, control_delay_, (model_.getMotorStatus().on && trigger) ? ros::WallDuration(1.0) : ros::WallDuration(), &callback_queue_); // fill input vector u for propulsion model geometry_msgs::Twist twist; fromVector(link->GetRelativeLinearVel(), twist.linear); fromVector(link->GetRelativeAngularVel(), twist.angular); model_.setTwist(twist); // update the model model_.update(dt.Double()); // get wrench from model Vector3 force, torque; toVector(model_.getWrench().force, force); toVector(model_.getWrench().torque, torque); // publish wrench if (wrench_publisher_) { geometry_msgs::WrenchStamped wrench_msg; wrench_msg.header.stamp = ros::Time(current_time.sec, current_time.nsec); wrench_msg.header.frame_id = frame_id_; wrench_msg.wrench = model_.getWrench(); wrench_publisher_.publish(wrench_msg); } // publish motor status if (motor_status_publisher_ && motorStatusTimer.update() /* && current_time >= last_motor_status_time_ + control_period_ */) { hector_uav_msgs::MotorStatus motor_status = model_.getMotorStatus(); motor_status.header.stamp = ros::Time(current_time.sec, current_time.nsec); motor_status_publisher_.publish(motor_status); last_motor_status_time_ = current_time; } // publish supply if (supply_publisher_ && current_time >= last_supply_time_ + 1.0) { supply_publisher_.publish(model_.getSupply()); last_supply_time_ = current_time; } // set force and torque in gazebo link->AddRelativeForce(force); link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force)); }