C++ (Cpp) fromVector Examples

Example #1

File: gazebo_quadrotor_aerodynamics.cpp Project: deriyuval/gazeboFilm

////////////////////////////////////////////////////////////////////////////////
// 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));
}

Example #2

File: ListNodeTest.cpp Project: proudzhu/leetcode

#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();
}

Example #3

File: gazebo_quadrotor_propulsion.cpp Project: hks95/quad-navigation-drl

////////////////////////////////////////////////////////////////////////////////
// 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));
}