search

uzh-rpg / flightmare

An Open Flexible Quadrotor Simulator
https://uzh-rpg.github.io/flightmare/
Other
977 stars 340 forks source link

Can't open Motion_planning example #149

Open michaeldepalma opened 2 years ago

michaeldepalma commented 2 years ago

When I execute "roslaunch flightros motion_planning.launch" the following error appears: "ERROR: cannot launch node of type [flightros/motion_planning]: Cannot locate node of type [motion_planning] in package [flightros]. Make sure file exists in package path and permission is set to executable (chmod +x)" I set executable permission for the node motion_planning.cpp (chmod +x motion_planning.cpp) in motion_planning folder and I checked the source path inside .bashrc : source /opt/ros/noetic/setup.bash source ~/catkin_ws/devel/setup.bash nevertheless the same error appears. I olso tried to change motion_planning.cpp inside code with racing.cpp code (that works!) and it still gives the same error. The problem is only with motion_planning example, the others (racing, pilot and camera) work perfectly.

This is motion_planning.launch file: 

<?xml version="1.0"?>
<launch>
    <arg name="debug" default="0" />
    <arg name="use_unity_editor" default="false" />

    <!-- RPG Flightmare Unity Render. -->
    <node pkg="flightrender" type="RPG_Flightmare.x86_64" name="rpg_flightmare_render" unless="$(arg use_unity_editor)">
    </node>

    <node name="motion_planning" pkg="flightros" type="motion_planning" output="screen" launch-prefix="gdb -ex run --args" 
       if="$(arg debug)" >
    </node>

    <node name="motion_planning" pkg="flightros" type="motion_planning" output="screen" unless="$(arg debug)">
    </node>
</launch>

Inside ~/catkin_ws/src/flightmare/flightros/src/motion_planning I have motion_planning.cpp

#include "flightros/motion_planning/motion_planning.hpp"

#define CONTROL_UPDATE_RATE 50.0

#define TINYPLY_IMPLEMENTATION

namespace ob = ompl::base;
namespace og = ompl::geometric;
using namespace tinyply;

std::vector<motion_planning::float3> motion_planning::readPointCloud() {
  std::unique_ptr<std::istream> file_stream;
  std::vector<uint8_t> byte_buffer;
  std::string filepath =
    std::experimental::filesystem::path(__FILE__).parent_path().string() +
    "/data/point_cloud.ply";
  try {
    file_stream.reset(new std::ifstream(filepath, std::ios::binary));

    if (!file_stream || file_stream->fail())
      throw std::runtime_error("file_stream failed to open " + filepath);

    file_stream->seekg(0, std::ios::end);
    const float size_mb = file_stream->tellg() * float(1e-6);
    file_stream->seekg(0, std::ios::beg);

    PlyFile file;
    file.parse_header(*file_stream);

    std::cout << "\t[ply_header] Type: "
              << (file.is_binary_file() ? "binary" : "ascii") << std::endl;
    for (const auto &c : file.get_comments())
      std::cout << "\t[ply_header] Comment: " << c << std::endl;
    for (const auto &c : file.get_info())
      std::cout << "\t[ply_header] Info: " << c << std::endl;

    for (const auto &e : file.get_elements()) {
      std::cout << "\t[ply_header] element: " << e.name << " (" << e.size << ")"
                << std::endl;
      for (const auto &p : e.properties) {
        std::cout << "\t[ply_header] \tproperty: " << p.name
                  << " (type=" << tinyply::PropertyTable[p.propertyType].str
                  << ")";
        if (p.isList)
          std::cout << " (list_type=" << tinyply::PropertyTable[p.listType].str
                    << ")";
        std::cout << std::endl;
      }
    }

    // Because most people have their own mesh types, tinyply treats parsed data
    // as structured/typed byte buffers. See examples below on how to marry your
    // own application-specific data structures with this one.
    std::shared_ptr<PlyData> vertices, normals, colors, texcoords, faces,
      tripstrip;

    // The header information can be used to programmatically extract properties
    // on elements known to exist in the header prior to reading the data. For
    // brevity of this sample, properties like vertex position are hard-coded:
    try {
      vertices =
        file.request_properties_from_element("vertex", {"x", "y", "z"});
    } catch (const std::exception &e) {
      std::cerr << "tinyply exception: " << e.what() << std::endl;
    }

    manual_timer read_timer;

    read_timer.start();
    file.read(*file_stream);
    read_timer.stop();

    const float parsing_time = read_timer.get() / 1000.f;
    std::cout << "\tparsing " << size_mb << "mb in " << parsing_time
              << " seconds [" << (size_mb / parsing_time) << " MBps]"
              << std::endl;

    if (vertices)
      std::cout << "\tRead " << vertices->count << " total vertices "
                << std::endl;

    const size_t numVerticesBytes = vertices->buffer.size_bytes();
    std::vector<float3> verts(vertices->count);
    std::memcpy(verts.data(), vertices->buffer.get(), numVerticesBytes);

    int idx = 0;
    for (const auto &point_tinyply : verts) {
      if (idx == 0) {
        points_ = Eigen::Vector3d(static_cast<double>(point_tinyply.x),
                                  static_cast<double>(point_tinyply.y),
                                  static_cast<double>(point_tinyply.z));
      } else {
        points_.conservativeResize(points_.rows(), points_.cols() + 1);
        points_.col(points_.cols() - 1) =
          Eigen::Vector3d(static_cast<double>(point_tinyply.x),
                          static_cast<double>(point_tinyply.y),
                          static_cast<double>(point_tinyply.z));
      }
      idx += 1;
    }
    kd_tree_.SetMatrixData(points_);

    return verts;
  } catch (const std::exception &e) {
    std::cerr << "Caught tinyply exception: " << e.what() << std::endl;
  }
  return verts;
}

void motion_planning::getBounds() {
  min_bounds = verts[0];
  max_bounds = verts[0];

  for (const auto &ver : verts) {
    if (ver.x < min_bounds.x) {
      min_bounds.x = ver.x;
    }

    if (ver.x > max_bounds.x) {
      max_bounds.x = ver.x;
    }

    if (ver.y < min_bounds.y) {
      min_bounds.y = ver.y;
    }

    if (ver.y > max_bounds.y) {
      max_bounds.y = ver.y;
    }

    if (ver.z > max_bounds.z) {
      max_bounds.z = ver.z;
    }
  }
}

void motion_planning::plan() {
  // construct the state space we are planning in
  auto space(std::make_shared<ob::SE3StateSpace>());

  // set the bounds for the R^3 part of SE(3)
  ob::RealVectorBounds bounds(3);

  bounds.setLow(0, min_bounds.x);
  bounds.setLow(1, min_bounds.y);
  bounds.setLow(2, min_bounds.z);
  bounds.setHigh(0, max_bounds.x);
  bounds.setHigh(1, max_bounds.y);
  bounds.setHigh(2, max_bounds.z);

  space->setBounds(bounds);

  // define a simple setup class
  og::SimpleSetup ss(space);

  // set state validity checking for this space
  ss.setStateValidityChecker(
    [](const ob::State *state) { return isStateValid(state); });

  // create a random start state
  ob::ScopedState<> start(space);
  start.random();

  // create a random goal state
  ob::ScopedState<> goal(space);
  goal.random();

  // set the start and goal states
  ss.setStartAndGoalStates(start, goal);

  // this call is optional, but we put it in to get more output information
  ss.setup();
  ss.print();

  // attempt to solve the problem within one second of planning time
  ob::PlannerStatus solved = ss.solve(1.0);

  if (solved) {
    std::cout << "Found solution:" << std::endl;

    ob::PlannerData pd(ss.getSpaceInformation());
    ss.getPlannerData(pd);
    /** backup cout buffer and redirect to path.txt **/
    std::ofstream out0(
      std::experimental::filesystem::path(__FILE__).parent_path().string() +
      "/data//vertices.txt");
    auto *coutbuf0 = std::cout.rdbuf();
    std::cout.rdbuf(out0.rdbuf());

    unsigned int num_vertices = pd.numVertices();
    for (unsigned int i = 0; i < num_vertices; i++) {
      Eigen::Vector3d p = stateToEigen(pd.getVertex(i).getState());
      std::cout << p.x() << " " << p.y() << " " << p.z() << " " << std::endl;
    }

    /** reset cout buffer **/
    std::cout.rdbuf(coutbuf0);

    /** backup cout buffer and redirect to path.txt **/
    std::ofstream out00(
      std::experimental::filesystem::path(__FILE__).parent_path().string() +
      "/data/edges.txt");
    auto *coutbuf00 = std::cout.rdbuf();
    std::cout.rdbuf(out00.rdbuf());

    for (unsigned int i = 0; i < num_vertices; i++) {
      std::vector<unsigned int> e;
      pd.getEdges(i, e);
      for (const auto &j : e) {
        std::cout << i << " " << j << " " << std::endl;
      }
    }

    /** reset cout buffer **/
    std::cout.rdbuf(coutbuf00);

    // save solution in solution_path.txt
    /** backup cout buffer and redirect to path.txt **/
    ss.simplifySolution();
    std::ofstream out(
      std::experimental::filesystem::path(__FILE__).parent_path().string() +
      "/data/solution_path.txt");
    auto *coutbuf = std::cout.rdbuf();
    std::cout.rdbuf(out.rdbuf());
    ss.getSolutionPath().printAsMatrix(std::cout);
    /** reset cout buffer **/
    std::cout.rdbuf(coutbuf);
    ss.getSolutionPath().print(std::cout);
    path_.clear();
    path_ = ss.getSolutionPath().getStates();
    vecs_.clear();
    for (auto const &pos : path_) {
      vecs_.push_back(stateToEigen(pos));
    }

    if (path_.size() > 1) {
      solution_found = true;
    }
  } else
    std::cout << "No solution found" << std::endl;
}

Eigen::Vector3d motion_planning::stateToEigen(const ompl::base::State *state) {
  // cast the abstract state type to the type we expect
  const auto *se3state = state->as<ob::SE3StateSpace::StateType>();

  // extract the first component of the state and cast it to what we expect
  const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);

  // extract the second component of the state and cast it to what we expect
  //  const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);

  // check validity of state defined by pos & rot
  float x = pos->values[0];
  float y = pos->values[1];
  float z = pos->values[2];

  // return a value that is always true but uses the two variables we define, so
  // we avoid compiler warnings
  // return isInRange(x, y, z);
  Eigen::Vector3d query_pos{x, y, z};
  return query_pos;
}

bool motion_planning::isStateValid(const ob::State *state) {
  // cast the abstract state type to the type we expect
  const auto *se3state = state->as<ob::SE3StateSpace::StateType>();

  // extract the first component of the state and cast it to what we expect
  const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);

  // extract the second component of the state and cast it to what we expect
  //  const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);

  // check validity of state defined by pos & rot
  float x = pos->values[0];
  float y = pos->values[1];
  float z = pos->values[2];
  // return a value that is always true but uses the two variables we define, so
  // we avoid compiler warnings
  // return isInRange(x, y, z);
  Eigen::Vector3d query_pos{x, y, z};
  return searchRadius(query_pos, range);
}

bool motion_planning::isInRange(float x, float y, float z) {
  bool validState = true;
  bool outOfBound = false;

  for (const auto &ver : verts) {
    // check if box around quad is occupied
    if (abs(ver.z - z) <= range) {
      if (abs(ver.x - x) <= range) {
        if (abs(ver.y - y) <= range) {
          validState = false;
        }
      }
    } else {
      if (ver.z - z >= 0) {
        outOfBound = true;
      }
    }

    if (outOfBound || !validState) {
      break;
    }
  }
  return validState;
}

bool motion_planning::searchRadius(const Eigen::Vector3d &query_point,
                                   const double radius) {
  std::vector<int> indices;
  std::vector<double> distances_squared;
  kd_tree_.SearchRadius(query_point, radius, indices, distances_squared);

  if (indices.size() == 0) {
    return true;
  }

  for (const auto &close_point_idx : indices) {
    // get point, check if within drone body
    Eigen::Vector3d close_point = points_.col(close_point_idx);
    // project point on each body axis and check distance
    Eigen::Vector3d close_point_body = (close_point - query_point);
    if (std::abs(close_point_body.x()) <= range &&
        std::abs(close_point_body.y()) <= range &&
        std::abs(close_point_body.z()) <= range) {
      // point is in collision
      return false;
    }
  }

  return true;
}

void motion_planning::executePath() {
  // initialization
  assert(!vecs_.empty());
  assert(vecs_.at(0).x() != NULL);

  // compute trajectory
  std::size_t num_waypoints = vecs_.size();
  int scaling = 10;

  std::vector<Eigen::Vector3d> way_points;

  for (int i = 0; i < int(num_waypoints - 1); i++) {
    Eigen::Vector3d diff_vec = vecs_.at(i + 1) - vecs_.at(i);
    double norm = diff_vec.norm();
    for (int j = 0; j < int(norm * scaling); j++) {
      way_points.push_back(vecs_.at(i) + j * (diff_vec / (norm * scaling)));
    }
  }

  // Flightmare
  Vector<3> B_r_BC(0.0, 0.0, 0.3);
  Matrix<3, 3> R_BC = Quaternion(1.0, 0.0, 0.0, 0.0).toRotationMatrix();
  std::cout << R_BC << std::endl;
  rgb_camera_->setFOV(90);
  rgb_camera_->setWidth(720);
  rgb_camera_->setHeight(480);
  rgb_camera_->setRelPose(B_r_BC, R_BC);
  quad_ptr_->addRGBCamera(rgb_camera_);

  quad_state_.setZero();
  quad_state_.x[QS::POSX] = (Scalar)vecs_.at(0).x();
  quad_state_.x[QS::POSY] = (Scalar)vecs_.at(0).y();
  quad_state_.x[QS::POSZ] = (Scalar)vecs_.at(0).z();

  quad_ptr_->reset(quad_state_);

  // connect unity
  setUnity(unity_render_);
  connectUnity();
  assert(unity_ready_);
  assert(unity_render_);

  int counter = 0;

  while (unity_render_ && unity_ready_) {
    quad_state_.x[QS::POSX] = (Scalar)way_points.at(counter).x();
    quad_state_.x[QS::POSY] = (Scalar)way_points.at(counter).y();
    quad_state_.x[QS::POSZ] = (Scalar)way_points.at(counter).z();
    quad_state_.x[QS::ATTW] = (Scalar)0;
    quad_state_.x[QS::ATTX] = (Scalar)0;
    quad_state_.x[QS::ATTY] = (Scalar)0;
    quad_state_.x[QS::ATTZ] = (Scalar)0;

    quad_ptr_->setState(quad_state_);

    unity_bridge_ptr_->getRender(0);
    unity_bridge_ptr_->handleOutput();

    counter += 1;
    counter = counter % way_points.size();
  }
}

bool motion_planning::setUnity(const bool render) {
  unity_render_ = render;
  if (unity_render_ && unity_bridge_ptr_ == nullptr) {
    // create unity bridge
    unity_bridge_ptr_ = UnityBridge::getInstance();
    unity_bridge_ptr_->addQuadrotor(quad_ptr_);
    std::cout << "Unity Bridge is created." << std::endl;
  }
  return true;
}

bool motion_planning::connectUnity() {
  if (!unity_render_ || unity_bridge_ptr_ == nullptr) return false;
  unity_ready_ = unity_bridge_ptr_->connectUnity(scene_id_);
  return unity_ready_;
}

int main(int argc, char *argv[]) {
  // initialize ROS
  ros::init(argc, argv, "flightmare_example");
  ros::NodeHandle nh("");
  ros::NodeHandle pnh("~");

  // quad initialization
  motion_planning::quad_ptr_ = std::make_unique<Quadrotor>();
  // add camera
  motion_planning::rgb_camera_ = std::make_unique<RGBCamera>();

  std::cout << "Read PointCloud" << std::endl;
  motion_planning::verts = motion_planning::readPointCloud();

  std::cout << "Get Bounds" << std::endl;
  motion_planning::getBounds();

  std::cout << "Plan & stuff" << std::endl;
  while (!motion_planning::solution_found) {
    motion_planning::plan();
  }

  std::cout << "Execute" << std::endl;
  motion_planning::executePath();

  return 0;
}

Inside "~/catkin_ws/src/flightmare/flightros/include/flightros/motion_planning" I have motion_planning.hpp and tinyply.h

motion_planning.hpp 

#pragma once

// standard libraries
#include <assert.h>
#include <Eigen/Dense>
#include <chrono>
#include <cmath>
#include <cstring>
#include <experimental/filesystem>
#include <fstream>
#include <iostream>
#include <iterator>
#include <sstream>
#include <thread>
#include <vector>

// Open3D
#include <Open3D/Geometry/KDTreeFlann.h>
#include <Open3D/Geometry/PointCloud.h>
#include <Open3D/IO/ClassIO/PointCloudIO.h>

// OMPL
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/config.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>

// TinyPly
#define TINYPLY_IMPLEMENTATION
#include "tinyply.h"

// flightlib
#include "flightlib/bridges/unity_bridge.hpp"
#include "flightlib/bridges/unity_message_types.hpp"
#include "flightlib/common/quad_state.hpp"
#include "flightlib/common/types.hpp"
#include "flightlib/objects/quadrotor.hpp"
#include "flightlib/sensors/rgb_camera.hpp"

// ros
#include <ros/ros.h>

namespace ob = ompl::base;
namespace og = ompl::geometric;

using namespace flightlib;

namespace motion_planning {

class manual_timer {
  std::chrono::high_resolution_clock::time_point t0;
  double timestamp{0.0};

 public:
  void start() { t0 = std::chrono::high_resolution_clock::now(); }
  void stop() {
    timestamp = std::chrono::duration<double>(
                  std::chrono::high_resolution_clock::now() - t0)
                  .count() *
                1000.0;
  }
  const double &get() { return timestamp; }
};

struct float2 {
  float x, y;
};
struct float3 {
  float x, y, z;
};
struct double3 {
  double x, y, z;
};
struct uint3 {
  uint32_t x, y, z;
};
struct uint4 {
  uint32_t x, y, z, w;
};
std::vector<float3> verts;
float range = 1;
bool solution_found = false;
bool trajectory_found = false;

std::vector<float3> readPointCloud();
float3 min_bounds;
float3 max_bounds;

Eigen::Vector3d stateToEigen(const ompl::base::State *state);

std::vector<ompl::base::State *> path_;
std::vector<Eigen::Vector3d> vecs_;

void getBounds();

void plan();

bool isStateValid(const ob::State *state);

bool isInRange(float x, float y, float z);

open3d::geometry::KDTreeFlann kd_tree_;
Eigen::MatrixXd points_;
bool searchRadius(const Eigen::Vector3d &query_point, const double radius);

void executePath();

// void setupQuad();
bool setUnity(const bool render);
bool connectUnity(void);

// unity quadrotor
std::shared_ptr<Quadrotor> quad_ptr_;
std::shared_ptr<RGBCamera> rgb_camera_;
QuadState quad_state_;

// Flightmare(Unity3D)
std::shared_ptr<UnityBridge> unity_bridge_ptr_;
SceneID scene_id_{UnityScene::NATUREFOREST};
bool unity_ready_{false};
bool unity_render_{true};
RenderMessage_t unity_output_;
uint16_t receive_id_{0};

}  // namespace motion_planning

I'll leave here the CMakeLists.txt of flightros folder: 


project(flightros)

cmake_minimum_required(VERSION 3.0.0)

find_package(catkin_simple REQUIRED)

find_package(OpenCV REQUIRED)

option(BUILD_MP "Build Motion Planning" OFF)

catkin_simple()

# Setup Default Build Type as Release
if (NOT CMAKE_BUILD_TYPE)
    set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE)
endif ()

# Setup Architcture-specific Flags
if ("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "armv7l")
    message(STATUS "Using ARMv7 optimized flags!")
    set(CMAKE_CXX_ARCH_FLAGS " -Wno-psabi -march=armv7-a -mfpu=neon -mfloat-abi=hard -funsafe-math-optimizations")
elseif ("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
    message(STATUS "Using ARM aarch64 optimized flags!")
    set(CMAKE_CXX_ARCH_FLAGS " -Wno-psabi -march=armv8-a+crypto -mcpu=cortex-a57+crypto")
else ()
    set(CMAKE_CXX_ARCH_FLAGS " -march=native")
endif ()

# Setup General C++ Flags
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DEIGEN_STACK_ALLOCATION_LIMIT=1048576")
# otherwise double free or corruption (out) error when running racing or motion_planning example
add_compile_options(-O3)

# Setup Release and Debug flags
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_ARCH_FLAGS} -Wall -DNDEBUG -fPIC")
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -Wall -g")

#pilot

cs_add_library(flight_pilot
    src/pilot/flight_pilot.cpp
)

target_link_libraries(flight_pilot
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
  stdc++fs
)

cs_add_executable(flight_pilot_node
   src/pilot/flight_pilot_node.cpp
)

target_link_libraries(flight_pilot_node
  flight_pilot
  ${OpenCV_LIBRARIES}
  stdc++fs
)

# motion_planning

if(BUILD_MP)

    find_package(OpenMP)
    if (OpenMP_CXX_FOUND)
        message("Found OpenMP ${OpenMP_CXX_FOUND}  ${OpenMP_VERSION} ${OpenMP_CXX_VERSION_MAJOR} ${Open3D_VERSION} OpenMP::OpenMP_CXX")
        get_cmake_property(_variableNames VARIABLES)
        list (SORT _variableNames)
        foreach (_variableName ${_variableNames})
            message(STATUS "${_variableName}=${${_variableName}}")
        endforeach()
    else ()
        message("OpenMP not found")
    endif ()

    # Open3D
    find_package(Open3D)
    if (Open3D_FOUND)
        message("Found Open3D ${Open3D_VERSION}")
        list(APPEND Open3D_LIBRARIES dl)
        # link_directories must be before add_executable
        link_directories(${Open3D_LIBRARY_DIRS})
    else ()
        message("Open3D not found")
    endif ()

    find_package(ompl REQUIRED)
    if (OMPL_FOUND)
        message("Found OMPL ${OMPL_VERSION}")
        include_directories(${OMPL_INCLUDE_DIRS})

     else ()
        message("OMPL not found")
    endif ()

    if (OpenMP_CXX_FOUND AND Open3D_FOUND AND OMPL_FOUND)
        cs_add_executable(motion_planning
            src/motion_planning/motion_planning.cpp
        )
        target_include_directories(motion_planning PUBLIC ${Open3D_INCLUDE_DIRS})

        target_link_libraries(motion_planning
        ${catkin_LIBRARIES}
        ${OpenCV_LIBRARIES}
        stdc++fs
        ompl
        ${Open3D_LIBRARIES}
        OpenMP::OpenMP_CXX
        zmq
        zmqpp
        )
    else ()
        message("Failed to build motion planning")
    endif ()

endif()

# racing

catkin_package(
LIBRARIES
CATKIN_DEPENDS
)

cs_add_executable(racing
    src/racing/racing.cpp
)

target_link_libraries(racing
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
stdc++fs
zmq
zmqpp
ompl
OpenMP::OpenMP_CXX
${Open3D_LIBRARIES}
)

# camera

cs_add_executable(camera
    src/camera/camera.cpp
)

target_link_libraries(camera
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
stdc++fs
zmq
zmqpp
)

# Finish
cs_install()
cs_export()
baj31415 commented 2 years ago

Hi, I have the same problem. Did you fix it?

wmy929 commented 1 year ago

Hi, I have the same problem. Did you fix it?