initial commit

.gitignore

@@ -0,0 +1,8 @@
+templates/*
+.orogen/*
+*~
+.*.swp
+build/*
+
+*autobuild-stamp
+

CMakeLists.txt

@@ -0,0 +1,9 @@
+PROJECT(nav)
+cmake_minimum_required(VERSION 2.6)
+
+SET (CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/.orogen/config")
+INCLUDE(navBase)
+
+# FIND_PACKAGE(KDL)
+# FIND_PACKAGE(OCL)
+

corridorWrappers.hpp

@@ -0,0 +1,54 @@
+#ifndef NAV_TYPES_H
+#define NAV_TYPES_H
+
+#include <base/wrappers/eigen.h>
+#include <base/wrappers/geometry/NURBSCurve3D.h>
+
+namespace nav
+{
+    class VoronoiPoint;
+    class Corridor;
+    class Plan;
+};
+
+namespace corridor_planner
+{
+    typedef wrappers::geometry::NURBSCurve3D Curve;
+    typedef std::vector<wrappers::Vector3> PointVector;
+
+    struct VoronoiPoint
+    {
+        double            width;
+        wrappers::Vector3 center;
+        PointVector       borders[2];
+    };
+
+    struct Corridor
+    {
+        std::vector<VoronoiPoint> voronoi;
+        PointVector boundaries[2];
+
+        Curve median_curve;
+        Curve boundary_curves[2];
+    };
+
+    enum CORRIDOR_SIDE
+    { FRONT_SIDE, BACK_SIDE };
+
+    struct CorridorConnection
+    {
+        int from_idx;
+        CORRIDOR_SIDE from_side;
+        int to_idx;
+        CORRIDOR_SIDE to_side;
+    };
+
+    struct Plan
+    {
+        std::vector<Corridor> corridors;
+        std::vector<CorridorConnection> connections;
+    };
+}
+
+#endif
+

corridor_planner.orogen

@@ -0,0 +1,23 @@
+name 'corridor_planner'
+
+using_library 'nav'
+import_types_from "base"
+import_types_from "corridorWrappers.hpp"
+
+task_context 'Task' do
+    needs_configuration
+    property 'terrain_classes', 'string'
+    property 'map', 'string'
+    property 'start_point',  'wrappers/Vector3'
+    property 'target_point', 'wrappers/Vector3'
+    property 'margin', 'double'
+
+    runtime_states :dstar, :skeleton, :plan, :plan_simplification
+
+    output_port 'plan', 'corridor_planner/Plan'
+end
+
+deployment 'test_corridor_planner' do
+    task('planner', 'Task')
+end
+

scripts/run

@@ -0,0 +1,66 @@
+#! /usr/bin/env ruby
+
+require 'orocos'
+include Orocos
+
+if ARGV.size != 7
+    STDERR.puts "scripts/run terrain_classes_file map_file goalX goalY startX startY expand_factor"
+    exit 1
+end
+
+BASE_DIR = File.expand_path('..', File.dirname(__FILE__))
+ENV['PKG_CONFIG_PATH'] = "#{File.join(BASE_DIR, 'build')}:#{ENV['PKG_CONFIG_PATH']}"
+Orocos.initialize
+
+terrain_classes_file = File.expand_path(ARGV.shift)
+map_file             = File.expand_path(ARGV.shift)
+if !File.exists?(terrain_classes_file)
+    STDERR.puts "#{terrain_classes_file} does not exist"
+end
+if !File.exists?(map_file)
+    STDERR.puts "#{map_file} does not exist"
+end
+
+goalX                = Float(ARGV.shift)
+goalY                = Float(ARGV.shift)
+startX               = Float(ARGV.shift)
+startY               = Float(ARGV.shift)
+expand_factor        = Float(ARGV.shift)
+
+Orocos::Process.spawn 'test_corridor_planner', :output => 'cplanner-log.txt' do |p|
+    task = p.task('planner')
+
+    task.terrain_classes = terrain_classes_file
+    task.map = map_file
+
+    start = task.start_point
+    start.data[0] = startX
+    start.data[1] = startY
+    start.data[2] = 0
+    task.start_point = start
+
+    goal = task.target_point
+    goal.data[0] = goalX
+    goal.data[1] = goalY
+    goal.data[2] = 0
+    task.target_point = goal
+
+    task.margin = expand_factor || 1.1
+
+    task.configure
+    task.start
+
+    last_state = nil
+    while true
+        if last_state != task.state
+            STDERR.puts task.state
+            last_state = task.state
+        end
+
+        if !task.running?
+            break
+        end
+        sleep 0.1
+    end
+end
+

scripts/run.sh

@@ -0,0 +1,6 @@
+#! /bin/sh
+
+HERE=`pwd`
+cd $1
+$HERE/run terrain_classes.txt terrain.tif 76 71 865 285 $2
+

tasks/CMakeLists.txt

@@ -0,0 +1,21 @@
+include(corridor_plannerTaskLib)
+ADD_LIBRARY(${CORRIDOR_PLANNER_TASKLIB_NAME} SHARED 
+    ${CORRIDOR_PLANNER_TASKLIB_SOURCES})
+
+add_dependencies(${CORRIDOR_PLANNER_TASKLIB_NAME}
+    regen-toolkit)
+
+
+TARGET_LINK_LIBRARIES(${CORRIDOR_PLANNER_TASKLIB_NAME}
+    ${OrocosRTT_LIBRARIES}
+    ${CORRIDOR_PLANNER_TASKLIB_DEPENDENT_LIBRARIES})
+SET_TARGET_PROPERTIES(${CORRIDOR_PLANNER_TASKLIB_NAME}
+    PROPERTIES LINK_INTERFACE_LIBRARIES "${CORRIDOR_PLANNER_TASKLIB_INTERFACE_LIBRARIES}")
+
+INSTALL(TARGETS ${CORRIDOR_PLANNER_TASKLIB_NAME}
+    RUNTIME DESTINATION bin
+    LIBRARY DESTINATION lib/orocos)
+
+INSTALL(FILES ${CORRIDOR_PLANNER_TASKLIB_HEADERS}
+    DESTINATION include/orocos/corridor_planner)
+

tasks/Task.cpp

@@ -0,0 +1,181 @@
+#include "Task.hpp"
+#include <nav/corridor_planner.hh>
+
+#include <boost/lexical_cast.hpp>
+
+using namespace corridor_planner;
+
+Task::Task(std::string const& name)
+    : TaskBase(name)
+    , planner(0) { }
+
+bool Task::configureHook()
+{
+    delete planner;
+    planner  = new nav::CorridorPlanner();
+    planner->init(_terrain_classes.get(), _map.get());
+
+    Eigen::Vector3d p0 = _start_point.get();
+    Eigen::Vector3d p1 = _target_point.get();
+    planner->setMarginFactor(_margin.get());
+    planner->setPositions(
+            Eigen::Vector2d(p0.x(), p0.y()), 
+            Eigen::Vector2d(p1.x(), p1.y()));
+
+    return true;
+}
+
+// bool Task::startHook()
+// {
+//     return true;
+// }
+
+template<typename SrcContainer, typename DstContainer>
+static void wrapContainer(DstContainer& dest, SrcContainer const& src,
+        double scale, Eigen::Transform3d const& world_to_local)
+{
+    dest.resize(src.size());
+
+    size_t point_idx;
+    typename SrcContainer::const_iterator src_it = src.begin();
+    typename SrcContainer::const_iterator const src_end = src.end();
+    for (point_idx = 0, src_it = src.begin(); src_it != src.end(); ++point_idx, ++src_it)
+    {
+        toWrapper(dest[point_idx], *src_it, scale, world_to_local);
+    }
+}
+
+template<typename SrcContainer>
+static void wrapContainer(std::vector< wrappers::Vector3 >& dest, SrcContainer const& src,
+        double scale, Eigen::Transform3d const& world_to_local)
+{
+    dest.reserve(src.size());
+
+    size_t point_idx;
+    typename SrcContainer::const_iterator src_it = src.begin();
+    typename SrcContainer::const_iterator const src_end = src.end();
+    for (point_idx = 0, src_it = src.begin(); src_it != src.end(); ++point_idx, ++src_it)
+    {
+        Eigen::Vector3d p(world_to_local * src_it->toEigen());
+        dest.push_back(p);
+    }
+}
+
+
+static void toWrapper(Curve& dest, base::geometry::NURBSCurve3D const& src,
+        double scale, Eigen::Transform3d const& world_to_local)
+{
+    dest.geometric_resolution = src.getGeometricResolution();
+    dest.curve_order = src.getCurveOrder();
+    std::vector<Eigen::Vector3d> const& points = src.getPoints();
+    for (int i = 0; i < points.size(); ++i)
+    {
+        Eigen::Vector3d p = world_to_local * points[i];
+        dest.points.push_back(p);
+    }
+}
+
+static void toWrapper(VoronoiPoint& dest, nav::VoronoiPoint const& src, 
+        double scale, Eigen::Transform3d const& world_to_local)
+{
+    dest.center = world_to_local * src.center.toEigen();
+    dest.width  = scale * src.width;
+
+    if (src.borders.size() != 2)
+        throw std::logic_error("got point with " + boost::lexical_cast<std::string>(src.borders.size()) + " borders");
+
+    wrapContainer(dest.borders[0], src.borders.front(), scale, world_to_local);
+    wrapContainer(dest.borders[1], src.borders.back(), scale, world_to_local);
+}
+
+static void toWrapper(Corridor& dest, nav::Corridor const& src,
+        double scale, Eigen::Transform3d const& world_to_local)
+{
+    wrapContainer(dest.voronoi, src.voronoi, scale, world_to_local);
+
+    dest.voronoi.resize(src.voronoi.size());
+    int i;
+    std::list< nav::VoronoiPoint>::const_iterator voronoi_it;
+    for (i = 0, voronoi_it = src.voronoi.begin(); voronoi_it != src.voronoi.end(); ++i, ++voronoi_it)
+        toWrapper(dest.voronoi[i], *voronoi_it, scale, world_to_local);
+
+    wrapContainer(dest.boundaries[0], src.boundaries[0], scale, world_to_local);
+    wrapContainer(dest.boundaries[1], src.boundaries[1], scale, world_to_local);
+
+    toWrapper(dest.median_curve, src.median_curve, scale, world_to_local);
+    toWrapper(dest.boundary_curves[0], src.boundary_curves[0], scale, world_to_local);
+    toWrapper(dest.boundary_curves[1], src.boundary_curves[1], scale, world_to_local);
+}
+
+static void toWrapper(Plan& dest, nav::Plan const& src,
+        double scale, Eigen::Transform3d const& world_to_local)
+{
+    wrapContainer(dest.corridors, src.corridors, scale, world_to_local);
+
+    for (int corridor_idx = 0; corridor_idx < src.corridors.size(); ++corridor_idx)
+    {
+        nav::Corridor const& corridor = src.corridors[corridor_idx];
+        nav::Corridor::const_connection_iterator
+            conn_it = corridor.connections.begin(),
+            conn_end = corridor.connections.end();
+
+        for (; conn_it != conn_end; ++conn_it)
+        {
+            CorridorConnection conn = 
+                { corridor_idx, conn_it->this_side ? BACK_SIDE : FRONT_SIDE,
+                  conn_it->target_idx, conn_it->target_side ? BACK_SIDE : FRONT_SIDE };
+
+            dest.connections.push_back(conn);
+        }
+    }
+}
+
+void Task::updateHook()
+{
+    if (state() == RUNNING)
+        state(DSTAR);
+
+    if (state() == DSTAR)
+    {
+        planner->computeDStar();
+        state(SKELETON);
+    }
+    else if (state() == SKELETON)
+    {
+        planner->extractSkeleton();
+        state(PLAN);
+    }
+    else if (state() == PLAN)
+    {
+        planner->computePlan();
+        state(PLAN_SIMPLIFICATION);
+    }
+    else if (state() == PLAN_SIMPLIFICATION)
+    {
+        planner->simplifyPlan();
+
+        // Finished, send to the output ports and stop the task
+        Plan result;
+        toWrapper(result, planner->plan, planner->map->getScale(),
+                planner->map->getLocalToWorld());
+        _plan.write(result);
+        stop();
+        return;
+    }
+
+    getActivity()->trigger();
+}
+
+// void Task::errorHook()
+// {
+// }
+
+// void Task::stopHook()
+// {
+//     delete map;
+//     map = 0;
+// }
+
+// void Task::cleanupHook()
+// {
+// }