diff --git a/.gitmodules b/.gitmodules index 0663916..499d385 100644 --- a/.gitmodules +++ b/.gitmodules @@ -10,3 +10,9 @@ [submodule "openvslam"] path = openvslam url = https://github.com/isro01/openvslam +[submodule "ObstacleAvoidance_ODGPF/petcat-obstacle-avoidance-"] + path = ObstacleAvoidance_ODGPF/petcat-obstacle-avoidance- + url = https://github.com/adit-khokar/petcat-obstacle-avoidance- +[submodule "Avoidance_Gaussian_Potential_Field/petcat-obstacle-avoidance-"] + path = Avoidance_Gaussian_Potential_Field/petcat-obstacle-avoidance- + url = https://github.com/adit-khokar/petcat-obstacle-avoidance- diff --git a/Avoidance_Gaussian_Potential_Field/CMakeLists.txt b/Avoidance_Gaussian_Potential_Field/CMakeLists.txt new file mode 100644 index 0000000..c994428 --- /dev/null +++ b/Avoidance_Gaussian_Potential_Field/CMakeLists.txt @@ -0,0 +1,218 @@ +cmake_minimum_required(VERSION 2.8.3) +project(husky_custom_navigation) +add_compile_options(-std=c++11) +find_package(PythonLibs 2.7) + + + +## Compile as C++11, supported in ROS Kinetic and newer + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + sensor_msgs + cv_bridge + image_transport + OpenCV REQUIRED +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES husky_custom_navigation +# CATKIN_DEPENDS roscpp std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/husky_custom_navigation.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/husky_custom_navigation_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_husky_custom_navigation.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) + + +add_executable(random_explorer random_explorer.cpp) + +target_include_directories(random_explorer PRIVATE ${Python_INCLUDE_DIRS}) +target_link_libraries(random_explorer ${catkin_LIBRARIES} ${PYTHON_LIBRARIES}) \ No newline at end of file diff --git a/Avoidance_Gaussian_Potential_Field/GaussianPF_PathPlanning.cpp b/Avoidance_Gaussian_Potential_Field/GaussianPF_PathPlanning.cpp new file mode 100644 index 0000000..feeee25 --- /dev/null +++ b/Avoidance_Gaussian_Potential_Field/GaussianPF_PathPlanning.cpp @@ -0,0 +1,81 @@ +# pragma once + +#include "ODG-PF.h" +#include +#include +#include +#include +#include +// #include "matplotlibcpp.h" +// namespace plt = matplotlibcpp; +using namespace std; + + + + +vector get_obstacles(vector polar_dat){ + vector obs; + int flag = 0; // represents whether an obs is being read + float theta1, theta2, angular_width, mean_angle, dist, d; + polar_dat[polar_dat.size()-1] = MAX_DIST; + for(int i=0;i=MAX_DIST)){ + flag = 0; + theta2 = i-1; + angular_width = theta2-theta1; + mean_angle = (theta2+theta1)/2; + d = accumulate(&polar_dat[theta1], &polar_dat[theta2], 0.0f) / angular_width; + assert(angular_width >= 0); + obs.push_back(obstacle(d, angular_width, mean_angle)); + theta1 = theta2; + } + } + + return obs; +} + +vector process_obs(vector &obs){ + vector obs_ = obs; + for (int i = 0; i potential){ + int header = std::distance(potential.begin(), std::min_element(potential.begin(), potential.end())); + return header; +} + +vector polar_data(360); // vector of size 360 : length of free path along each dir +vector potential(360); // Final potential at each angle + +float get_header_rad(vector polardat, float goal_angle, bool showPlot){ + float header; + + auto obstacles = get_obstacles(polardat); + // obstacles = process_obs(obstacles); + // For debugging + if(1){ + for(auto a:obstacles){ + cout << "Obstacle is " << a.get_dist()<< "m away at angle "< input[360]; + + +//sample output array for storing potential field values +vector field[360]; + +float index_to_angle(int i){ + return (PI*i)/180; +} + +float angle_to_index(float a){ + return (a*180)/PI; +} + +void draw_circle(float cx, float cy, float rad){ + vector x; + vector y; + for(int i =0;i<360;i++){ + float a = index_to_angle(i); + x.push_back(cx+rad*cos(a)); + y.push_back(cy+rad*sin(a)); + } + // plt::plot(x, y); +} + +class obstacle +{ +private: + float d; //average distance to each obstacle + float phi; //the angle occupied by it + // float sigma; //half of the angle occupied by the obstacle considering for size of bot + float theta; //center angle for obstacle + float A ; + +public: + obstacle(float d_in, float phi_in,float theta_in); //constructor + void increase_width(float w); // + void compute_field(vector& field); //compute and add field to the total field + float get_theta(){ return theta;} + float get_dist(){return d;} + float get_phi(){ return phi;} + bool inVic(); +}; + +obstacle::obstacle(float d_in, float phi_in,float theta_in) +{ + d = d_in; + phi = phi_in; + theta = theta_in; + A = (MAX_DIST-d)*exp(0.5); +} + +void obstacle::increase_width(float w){ + float phi_ = index_to_angle(phi); + phi_ = 2*atan((tan(phi_/2)+ (w/2) )/d ); + phi_ = angle_to_index(phi_); + this->phi = phi_; +} + +void obstacle::compute_field(vector &field) +{ + float theta = index_to_angle(this->theta); + float ph = index_to_angle(this->phi); + for(int i=0;i < field.size();i++){ + float angle = index_to_angle(i); + double temp = A*exp(-1*((theta-angle)*(theta-angle))/(2*ph*ph)); + field[i] += temp; + } +} + +bool obstacle::inVic(){ + return (d<=0.15); +} + +float get_goal_angle(float x, float y, float goal_x, float goal_y){ + float a = atan((goal_y-y)/(goal_x-x)); + if(goal_y-y<0 && goal_x-x>0){ + a = 2*PI + a; + } + else if(goal_y-y<0 && goal_x-x<0){ + a = PI + a; + } + else if(goal_y-y>0 && goal_x-x<0){ + a = PI+ a; + } + return a; +} + +void goal_field(vector &field, float goal_angle){ + for (int i = 0; i < field.size(); i++) + { + float angle = index_to_angle(i); + auto temp = (Gamma)*abs(angle - goal_angle); + field[i] += temp; + } + +} + +class rect{ + float x1, x2, x3, x4; + float y1, y2, y3, y4; + public: + rect(float x1_, float y1_, float x3_, float y3_){ + x1 = x1_; + x2 = x3_; + x3 = x3_; + x4 = x1_; + y1 = y1_; + y2 = y1_; + y3 = y3_; + y4 = y3_; + } + + bool on_line(float x, float y); + float get_min_dist(float x, float y, float theta); + void draw(); + +}; + +bool rect::on_line(float x, float y){ + float cx = (x1+x2)/2; + float cy = (y1+y4)/2; + float rad = abs(x1-x2)/2; + if((x-cx)*(x-cx)+ (y-cy)*(y-cy) <= rad*rad) + return true; + + return false; +} + +float rect::get_min_dist(float x, float y, float theta){ + float d = 0; + if(this->on_line(x, y)){ + float cx = (this->x1+this->x2)/2; + float cy = (this->y1+this->y4)/2; + float atoc = get_goal_angle(x, y,cx, cy); + return abs(theta-atoc); + } + for(d = 0; d < MAX_DIST; d+=0.001){ + float xi, yi; + xi = x + d*cos(theta); + yi = y + d*sin(theta); + + if(this->on_line(xi, yi)) + break; + + } + return d; +} + +void rect::draw(){ + draw_circle((x1+x2)/2, (y1+y3)/2, abs(x2-x1)/2); +} + +float mapx(float current, float goal, float max){ + return max*abs(current-goal); +} diff --git a/Avoidance_Gaussian_Potential_Field/README.md b/Avoidance_Gaussian_Potential_Field/README.md new file mode 100644 index 0000000..fbc4c7b --- /dev/null +++ b/Avoidance_Gaussian_Potential_Field/README.md @@ -0,0 +1,249 @@ +# Obstacle Avoidance with Gaussian Potential Field Method + +## Algorithm + +The main idea behind this method is to + +1. Receive distance data from the range sensor(s). +2. Consider only the objects that are within the threshold range. +3. Enlarge the obstacles with regard to the vehicle’s width. +4. Construct a Gaussian (repulsive) potential field from them. +5. Calculate the attractive field from the yaw angle information from an inertial measurement unit (IMU). +6. The total field is made of these two fields. Choose the angle with the minimum total field value. + +The algorithm has been tested on a husky, the node for which can be found [here](random_explorer.cpp). + +## Modules + +### ODG-PF.h + +```cpp +#include "ODG-PF.h" +``` + +#### Global Constants + +```cpp +const float Gamma = 0.59 +const float LC = 1 +const float MAX_DIST = 3.5 +``` + +#### Functions + +``` cpp +float get_goal_angle(float x, float y, float goal_x, float goal_y) +void goal_field(vector &field, float goal_angle) +float index_to_angle(int i) +float angle_to_index(float a) +``` + +#### Description + +1. **get_goal_angle()** + +```cpp +float get_goal_angle( float x, + float y, + float goal_x, + float goal_y ) +``` + +Returns the goal angle in radians given the ego and goal positions + +*Parameters*: + +* **x** - Ego x coordinate +* **y** - Ego y coordinate +* **goal_x** - Goal’s x coordinate +* **goal_y** - Goal’s y coordinate + +2. **goal_field()** + +```cpp +void goal_field( vector &field, + float goal_angle ) +``` + +Adds the attractive goal field to the potential field + +*Parameters*: + +* **field** - The potential field +* **goal_angle** - Goal angle in radians + +3. **index_to_angle()** + +```cpp +float index_to_angle(int i) +``` + +Returns the angle in radians for index **i** + +4. **angle_to_index()** + +```cpp +float angle_to_index(float a) +``` + +Returns the index value for angle **a**(radians) + +#### Classes + +1. **obstacle** + +**Private Data Members** + +```cpp +float d +float phi +float theta +float A +``` + +**Public Member Functions** + +```cpp +obstacle(float d_in, float phi_in,float theta_in) +void increase_width(float w) +void compute_field(vector& field) +float get_theta() +float get_dist() +float get_phi() +``` + +**Detailed Description** + +1. **obstacle()** + +```cpp +obstacle( float d_in, + float phi_in, + float theta_in ) +``` + +Instantiates the obstacle object. + +*Parameters*: + +* **d_in** - Mean distance +* **phi_in** - Angular width +* **theta_in** - Mean angle of the obstacle + +2. **increase_width()** + +```cpp +void increase_width(float w) +``` + +Increases the width of all obstacles as per the bot’s width. + +*Parameters*: + +* **w** - Width of the bot + +3. **compute_field()** + +```cpp +void compute_field(vector& field) +``` + +Computes the Gaussian potential field due to this obstacle. + +*Parameters*: + +* **field** - Potential field to which this obstacles contribution is added + +4. **get_theta()** + +```cpp +float get_theta() +``` + +Returns the angular position of the obstacle. + +5. **get_dist()** + +```cpp +float get_dist() +``` + +Returns the distance of the obstacle from the bot. + +5. **get_phi()** + +```cpp +float get_phi() +``` + +Returns the angular width of the obstacle. + +### GaussianPF_PathPlanning.cpp + +```cpp +#include "GaussianPF_PathPlanning.cpp" +``` + +This module uses the OBD-GF header file for implementing the algorithm. + +#### Methods + +```cpp +vector get_obstacles(vector polar_dat) +vector process_obs(vector &obs) +int get_best_header(vector potential) +float get_header_rad(vector polardat, float goal_angle, bool showPlot) +``` + +#### Description + +1. **get_obstacles()** + +```cpp +vector get_obstacles(vector polar_dat) +``` + +Returns an array of obstacle instances for the given lidar scan data. + +*Parameters:* + +* **polar_dat** - Lidar scan data (array of distances) + +2. **process_obs()** + +```cpp +vector process_obs(vector &obs) +``` + +Augments the obstacle array by increasing the angular width of each obstacle as per the bot's width. + +*Parameters*: +* **obs** - Array of obstacles obtained from get_obstacles() + +3. **get_best_header()** + +```cpp +int get_best_header(vector potential) +``` + +Returns the angle with minimum potential given the potential field. + +*Parameters:* + +* **potential** - Potential field array : value of potential at the index angle + +4. **get_header_rad()** + +```cpp +get_header_rad( vector polardat, + float goal_angle, + bool showPlot) +``` + +Returns the angle of minimum potential in radians given the lidar scan data. + +*Parameters* : + +* **polardat** - Lidar scan data +* **goal_angle** - The desired heading angle towards the goal +* **show_plot** - Determines whether potential field plots are shown diff --git a/Avoidance_Gaussian_Potential_Field/matplotlibcpp.h b/Avoidance_Gaussian_Potential_Field/matplotlibcpp.h new file mode 100644 index 0000000..0112344 --- /dev/null +++ b/Avoidance_Gaussian_Potential_Field/matplotlibcpp.h @@ -0,0 +1,2212 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include // requires c++11 support +#include +#include + +#include + +#ifndef WITHOUT_NUMPY +# define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +# include + +# ifdef WITH_OPENCV +# include +# endif // WITH_OPENCV + +/* + * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so + * define the ones we need here. + */ +# if CV_MAJOR_VERSION > 3 +# define CV_BGR2RGB cv::COLOR_BGR2RGB +# define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA +# endif +#endif // WITHOUT_NUMPY + +#if PY_MAJOR_VERSION >= 3 +# define PyString_FromString PyUnicode_FromString +# define PyInt_FromLong PyLong_FromLong +# define PyString_FromString PyUnicode_FromString +#endif + + +namespace matplotlibcpp { +namespace detail { + +static std::string s_backend; + +struct _interpreter { + PyObject *s_python_function_show; + PyObject *s_python_function_close; + PyObject *s_python_function_draw; + PyObject *s_python_function_pause; + PyObject *s_python_function_save; + PyObject *s_python_function_figure; + PyObject *s_python_function_fignum_exists; + PyObject *s_python_function_plot; + PyObject *s_python_function_quiver; + PyObject *s_python_function_semilogx; + PyObject *s_python_function_semilogy; + PyObject *s_python_function_loglog; + PyObject *s_python_function_fill; + PyObject *s_python_function_fill_between; + PyObject *s_python_function_hist; + PyObject *s_python_function_imshow; + PyObject *s_python_function_scatter; + PyObject *s_python_function_boxplot; + PyObject *s_python_function_subplot; + PyObject *s_python_function_subplot2grid; + PyObject *s_python_function_legend; + PyObject *s_python_function_xlim; + PyObject *s_python_function_ion; + PyObject *s_python_function_ginput; + PyObject *s_python_function_ylim; + PyObject *s_python_function_title; + PyObject *s_python_function_axis; + PyObject *s_python_function_axvline; + PyObject *s_python_function_xlabel; + PyObject *s_python_function_ylabel; + PyObject *s_python_function_gca; + PyObject *s_python_function_xticks; + PyObject *s_python_function_yticks; + PyObject *s_python_function_tick_params; + PyObject *s_python_function_grid; + PyObject *s_python_function_clf; + PyObject *s_python_function_errorbar; + PyObject *s_python_function_annotate; + PyObject *s_python_function_tight_layout; + PyObject *s_python_colormap; + PyObject *s_python_empty_tuple; + PyObject *s_python_function_stem; + PyObject *s_python_function_xkcd; + PyObject *s_python_function_text; + PyObject *s_python_function_suptitle; + PyObject *s_python_function_bar; + PyObject *s_python_function_colorbar; + PyObject *s_python_function_subplots_adjust; + + + /* For now, _interpreter is implemented as a singleton since its currently not possible to have + multiple independent embedded python interpreters without patching the python source code + or starting a separate process for each. + http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program + */ + + static _interpreter& get() { + static _interpreter ctx; + return ctx; + } + + PyObject* safe_import(PyObject* module, std::string fname) { + PyObject* fn = PyObject_GetAttrString(module, fname.c_str()); + + if (!fn) + throw std::runtime_error(std::string("Couldn't find required function: ") + fname); + + if (!PyFunction_Check(fn)) + throw std::runtime_error(fname + std::string(" is unexpectedly not a PyFunction.")); + + return fn; + } + +private: + +#ifndef WITHOUT_NUMPY +# if PY_MAJOR_VERSION >= 3 + + void *import_numpy() { + import_array(); // initialize C-API + return NULL; + } + +# else + + void import_numpy() { + import_array(); // initialize C-API + } + +# endif +#endif + + _interpreter() { + + // optional but recommended +#if PY_MAJOR_VERSION >= 3 + wchar_t name[] = L"plotting"; +#else + char name[] = "plotting"; +#endif + Py_SetProgramName(name); + Py_Initialize(); + +#ifndef WITHOUT_NUMPY + import_numpy(); // initialize numpy C-API +#endif + + PyObject* matplotlibname = PyString_FromString("matplotlib"); + PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); + PyObject* cmname = PyString_FromString("matplotlib.cm"); + PyObject* pylabname = PyString_FromString("pylab"); + if (!pyplotname || !pylabname || !matplotlibname || !cmname) { + throw std::runtime_error("couldnt create string"); + } + + PyObject* matplotlib = PyImport_Import(matplotlibname); + Py_DECREF(matplotlibname); + if (!matplotlib) { + PyErr_Print(); + throw std::runtime_error("Error loading module matplotlib!"); + } + + // matplotlib.use() must be called *before* pylab, matplotlib.pyplot, + // or matplotlib.backends is imported for the first time + if (!s_backend.empty()) { + PyObject_CallMethod(matplotlib, const_cast("use"), const_cast("s"), s_backend.c_str()); + } + + PyObject* pymod = PyImport_Import(pyplotname); + Py_DECREF(pyplotname); + if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } + + s_python_colormap = PyImport_Import(cmname); + Py_DECREF(cmname); + if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); } + + PyObject* pylabmod = PyImport_Import(pylabname); + Py_DECREF(pylabname); + if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); } + + s_python_function_show = safe_import(pymod, "show"); + s_python_function_close = safe_import(pymod, "close"); + s_python_function_draw = safe_import(pymod, "draw"); + s_python_function_pause = safe_import(pymod, "pause"); + s_python_function_figure = safe_import(pymod, "figure"); + s_python_function_fignum_exists = safe_import(pymod, "fignum_exists"); + s_python_function_plot = safe_import(pymod, "plot"); + s_python_function_quiver = safe_import(pymod, "quiver"); + s_python_function_semilogx = safe_import(pymod, "semilogx"); + s_python_function_semilogy = safe_import(pymod, "semilogy"); + s_python_function_loglog = safe_import(pymod, "loglog"); + s_python_function_fill = safe_import(pymod, "fill"); + s_python_function_fill_between = safe_import(pymod, "fill_between"); + s_python_function_hist = safe_import(pymod,"hist"); + s_python_function_scatter = safe_import(pymod,"scatter"); + s_python_function_boxplot = safe_import(pymod,"boxplot"); + s_python_function_subplot = safe_import(pymod, "subplot"); + s_python_function_subplot2grid = safe_import(pymod, "subplot2grid"); + s_python_function_legend = safe_import(pymod, "legend"); + s_python_function_ylim = safe_import(pymod, "ylim"); + s_python_function_title = safe_import(pymod, "title"); + s_python_function_axis = safe_import(pymod, "axis"); + s_python_function_axvline = safe_import(pymod, "axvline"); + s_python_function_xlabel = safe_import(pymod, "xlabel"); + s_python_function_ylabel = safe_import(pymod, "ylabel"); + s_python_function_gca = safe_import(pymod, "gca"); + s_python_function_xticks = safe_import(pymod, "xticks"); + s_python_function_yticks = safe_import(pymod, "yticks"); + s_python_function_tick_params = safe_import(pymod, "tick_params"); + s_python_function_grid = safe_import(pymod, "grid"); + s_python_function_xlim = safe_import(pymod, "xlim"); + s_python_function_ion = safe_import(pymod, "ion"); + s_python_function_ginput = safe_import(pymod, "ginput"); + s_python_function_save = safe_import(pylabmod, "savefig"); + s_python_function_annotate = safe_import(pymod,"annotate"); + s_python_function_clf = safe_import(pymod, "clf"); + s_python_function_errorbar = safe_import(pymod, "errorbar"); + s_python_function_tight_layout = safe_import(pymod, "tight_layout"); + s_python_function_stem = safe_import(pymod, "stem"); + s_python_function_xkcd = safe_import(pymod, "xkcd"); + s_python_function_text = safe_import(pymod, "text"); + s_python_function_suptitle = safe_import(pymod, "suptitle"); + s_python_function_bar = safe_import(pymod,"bar"); + s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar"); + s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust"); +#ifndef WITHOUT_NUMPY + s_python_function_imshow = safe_import(pymod, "imshow"); +#endif + s_python_empty_tuple = PyTuple_New(0); + } + + ~_interpreter() { + Py_Finalize(); + } +}; + +} // end namespace detail + +/// Select the backend +/// +/// **NOTE:** This must be called before the first plot command to have +/// any effect. +/// +/// Mainly useful to select the non-interactive 'Agg' backend when running +/// matplotlibcpp in headless mode, for example on a machine with no display. +/// +/// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use +inline void backend(const std::string& name) +{ + detail::s_backend = name; +} + +inline bool annotate(std::string annotation, double x, double y) +{ + PyObject * xy = PyTuple_New(2); + PyObject * str = PyString_FromString(annotation.c_str()); + + PyTuple_SetItem(xy,0,PyFloat_FromDouble(x)); + PyTuple_SetItem(xy,1,PyFloat_FromDouble(y)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "xy", xy); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +namespace detail { + +#ifndef WITHOUT_NUMPY +// Type selector for numpy array conversion +template struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default +template <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; + +template +PyObject* get_array(const std::vector& v) +{ + detail::_interpreter::get(); //interpreter needs to be initialized for the numpy commands to work + NPY_TYPES type = select_npy_type::type; + if (type == NPY_NOTYPE) + { + std::vector vd(v.size()); + npy_intp vsize = v.size(); + std::copy(v.begin(),v.end(),vd.begin()); + PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, NPY_DOUBLE, (void*)(vd.data())); + return varray; + } + + npy_intp vsize = v.size(); + PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); + return varray; +} + +template +PyObject* get_2darray(const std::vector<::std::vector>& v) +{ + detail::_interpreter::get(); //interpreter needs to be initialized for the numpy commands to work + if (v.size() < 1) throw std::runtime_error("get_2d_array v too small"); + + npy_intp vsize[2] = {static_cast(v.size()), + static_cast(v[0].size())}; + + PyArrayObject *varray = + (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE); + + double *vd_begin = static_cast(PyArray_DATA(varray)); + + for (const ::std::vector &v_row : v) { + if (v_row.size() != static_cast(vsize[1])) + throw std::runtime_error("Missmatched array size"); + std::copy(v_row.begin(), v_row.end(), vd_begin); + vd_begin += vsize[1]; + } + + return reinterpret_cast(varray); +} + +#else // fallback if we don't have numpy: copy every element of the given vector + +template +PyObject* get_array(const std::vector& v) +{ + PyObject* list = PyList_New(v.size()); + for(size_t i = 0; i < v.size(); ++i) { + PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i))); + } + return list; +} + +#endif // WITHOUT_NUMPY + +// sometimes, for labels and such, we need string arrays +inline PyObject * get_array(const std::vector& strings) +{ + PyObject* list = PyList_New(strings.size()); + for (std::size_t i = 0; i < strings.size(); ++i) { + PyList_SetItem(list, i, PyString_FromString(strings[i].c_str())); + } + return list; +} + +// not all matplotlib need 2d arrays, some prefer lists of lists +template +PyObject* get_listlist(const std::vector>& ll) +{ + PyObject* listlist = PyList_New(ll.size()); + for (std::size_t i = 0; i < ll.size(); ++i) { + PyList_SetItem(listlist, i, get_array(ll[i])); + } + return listlist; +} + +} // namespace detail + +/// Plot a line through the given x and y data points.. +/// +/// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html +template +bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +// TODO - it should be possible to make this work by implementing +// a non-numpy alternative for `detail::get_2darray()`. +#ifndef WITHOUT_NUMPY +template +void plot_surface(const std::vector<::std::vector> &x, + const std::vector<::std::vector> &y, + const std::vector<::std::vector> &z, + const std::map &keywords = + std::map()) +{ + // We lazily load the modules here the first time this function is called + // because I'm not sure that we can assume "matplotlib installed" implies + // "mpl_toolkits installed" on all platforms, and we don't want to require + // it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + assert(x.size() == y.size()); + assert(y.size() == z.size()); + + // using numpy arrays + PyObject *xarray = detail::get_2darray(x); + PyObject *yarray = detail::get_2darray(y); + PyObject *zarray = detail::get_2darray(z); + + // construct positional args + PyObject *args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + PyTuple_SetItem(args, 2, zarray); + + // Build up the kw args. + PyObject *kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1)); + PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1)); + + PyObject *python_colormap_coolwarm = PyObject_GetAttrString( + detail::_interpreter::get().s_python_colormap, "coolwarm"); + + PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); + + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + + PyObject *fig = + PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + if (!fig) throw std::runtime_error("Call to figure() failed."); + + PyObject *gca_kwargs = PyDict_New(); + PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); + + PyObject *gca = PyObject_GetAttrString(fig, "gca"); + if (!gca) throw std::runtime_error("No gca"); + Py_INCREF(gca); + PyObject *axis = PyObject_Call( + gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); + + if (!axis) throw std::runtime_error("No axis"); + Py_INCREF(axis); + + Py_DECREF(gca); + Py_DECREF(gca_kwargs); + + PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface"); + if (!plot_surface) throw std::runtime_error("No surface"); + Py_INCREF(plot_surface); + PyObject *res = PyObject_Call(plot_surface, args, kwargs); + if (!res) throw std::runtime_error("failed surface"); + Py_DECREF(plot_surface); + + Py_DECREF(axis); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} +#endif // WITHOUT_NUMPY + +template +void plot3(const std::vector &x, + const std::vector &y, + const std::vector &z, + const std::map &keywords = + std::map()) +{ + // Same as with plot_surface: We lazily load the modules here the first time + // this function is called because I'm not sure that we can assume "matplotlib + // installed" implies "mpl_toolkits installed" on all platforms, and we don't + // want to require it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + assert(x.size() == y.size()); + assert(y.size() == z.size()); + + PyObject *xarray = detail::get_array(x); + PyObject *yarray = detail::get_array(y); + PyObject *zarray = detail::get_array(z); + + // construct positional args + PyObject *args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + PyTuple_SetItem(args, 2, zarray); + + // Build up the kw args. + PyObject *kwargs = PyDict_New(); + + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject *fig = + PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + if (!fig) throw std::runtime_error("Call to figure() failed."); + + PyObject *gca_kwargs = PyDict_New(); + PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); + + PyObject *gca = PyObject_GetAttrString(fig, "gca"); + if (!gca) throw std::runtime_error("No gca"); + Py_INCREF(gca); + PyObject *axis = PyObject_Call( + gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); + + if (!axis) throw std::runtime_error("No axis"); + Py_INCREF(axis); + + Py_DECREF(gca); + Py_DECREF(gca_kwargs); + + PyObject *plot3 = PyObject_GetAttrString(axis, "plot"); + if (!plot3) throw std::runtime_error("No 3D line plot"); + Py_INCREF(plot3); + PyObject *res = PyObject_Call(plot3, args, kwargs); + if (!res) throw std::runtime_error("Failed 3D line plot"); + Py_DECREF(plot3); + + Py_DECREF(axis); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} + +template +bool stem(const std::vector &x, const std::vector &y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = + keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_stem, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) + Py_DECREF(res); + + return res; +} + +template< typename Numeric > +bool fill(const std::vector& x, const std::vector& y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if (res) Py_DECREF(res); + + return res; +} + +template< typename Numeric > +bool fill_between(const std::vector& x, const std::vector& y1, const std::vector& y2, const std::map& keywords) +{ + assert(x.size() == y1.size()); + assert(x.size() == y2.size()); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* y1array = detail::get_array(y1); + PyObject* y2array = detail::get_array(y2); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, y1array); + PyTuple_SetItem(args, 2, y2array); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template< typename Numeric> +bool hist(const std::vector& y, long bins=10,std::string color="b", + double alpha=1.0, bool cumulative=false) +{ + + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + PyDict_SetItemString(kwargs, "cumulative", cumulative ? Py_True : Py_False); + + PyObject* plot_args = PyTuple_New(1); + + PyTuple_SetItem(plot_args, 0, yarray); + + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +#ifndef WITHOUT_NUMPY +namespace detail { + +inline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map &keywords, PyObject** out) +{ + assert(type == NPY_UINT8 || type == NPY_FLOAT); + assert(colors == 1 || colors == 3 || colors == 4); + + detail::_interpreter::get(); //interpreter needs to be initialized for the numpy commands to work + + // construct args + npy_intp dims[3] = { rows, columns, colors }; + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyArray_SimpleNewFromData(colors == 1 ? 2 : 3, dims, type, ptr)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_imshow, args, kwargs); + Py_DECREF(args); + Py_DECREF(kwargs); + if (!res) + throw std::runtime_error("Call to imshow() failed"); + if (out) + *out = res; + else + Py_DECREF(res); +} + +} // namespace detail + +inline void imshow(const unsigned char *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) +{ + detail::imshow((void *) ptr, NPY_UINT8, rows, columns, colors, keywords, out); +} + +inline void imshow(const float *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) +{ + detail::imshow((void *) ptr, NPY_FLOAT, rows, columns, colors, keywords, out); +} + +#ifdef WITH_OPENCV +void imshow(const cv::Mat &image, const std::map &keywords = {}) +{ + // Convert underlying type of matrix, if needed + cv::Mat image2; + NPY_TYPES npy_type = NPY_UINT8; + switch (image.type() & CV_MAT_DEPTH_MASK) { + case CV_8U: + image2 = image; + break; + case CV_32F: + image2 = image; + npy_type = NPY_FLOAT; + break; + default: + image.convertTo(image2, CV_MAKETYPE(CV_8U, image.channels())); + } + + // If color image, convert from BGR to RGB + switch (image2.channels()) { + case 3: + cv::cvtColor(image2, image2, CV_BGR2RGB); + break; + case 4: + cv::cvtColor(image2, image2, CV_BGRA2RGBA); + } + + detail::imshow(image2.data, npy_type, image2.rows, image2.cols, image2.channels(), keywords); +} +#endif // WITH_OPENCV +#endif // WITHOUT_NUMPY + +template +bool scatter(const std::vector& x, + const std::vector& y, + const double s=1.0, // The marker size in points**2 + const std::unordered_map & keywords = {}) +{ + assert(x.size() == y.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s)); + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool boxplot(const std::vector>& data, + const std::vector& labels = {}, + const std::unordered_map & keywords = {}) +{ + PyObject* listlist = detail::get_listlist(data); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, listlist); + + PyObject* kwargs = PyDict_New(); + + // kwargs needs the labels, if there are (the correct number of) labels + if (!labels.empty() && labels.size() == data.size()) { + PyDict_SetItemString(kwargs, "labels", detail::get_array(labels)); + } + + // take care of the remaining keywords + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +template +bool boxplot(const std::vector& data, + const std::unordered_map & keywords = {}) +{ + PyObject* vector = detail::get_array(data); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, vector); + + PyObject* kwargs = PyDict_New(); + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +template +bool bar(const std::vector & x, + const std::vector & y, + std::string ec = "black", + std::string ls = "-", + double lw = 1.0, + const std::map & keywords = {}) { + PyObject * xarray = detail::get_array(x); + PyObject * yarray = detail::get_array(y); + + PyObject * kwargs = PyDict_New(); + + PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); + PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); + PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); + + for (std::map::const_iterator it = + keywords.begin(); + it != keywords.end(); + ++it) { + PyDict_SetItemString( + kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject * plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject * res = PyObject_Call( + detail::_interpreter::get().s_python_function_bar, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); + + return res; +} + +template +bool bar(const std::vector & y, + std::string ec = "black", + std::string ls = "-", + double lw = 1.0, + const std::map & keywords = {}) { + using T = typename std::remove_reference::type::value_type; + + std::vector x; + for (std::size_t i = 0; i < y.size(); i++) { x.push_back(i); } + + return bar(x, y, ec, ls, lw, keywords); +} + +inline bool subplots_adjust(const std::map& keywords = {}) +{ + + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = + keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyFloat_FromDouble(it->second)); + } + + + PyObject* plot_args = PyTuple_New(0); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_subplots_adjust, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template< typename Numeric> +bool named_hist(std::string label,const std::vector& y, long bins=10, std::string color="b", double alpha=1.0) +{ + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str())); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + + + PyObject* plot_args = PyTuple_New(1); + PyTuple_SetItem(plot_args, 0, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool plot(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool quiver(const std::vector& x, const std::vector& y, const std::vector& u, const std::vector& w, const std::map& keywords = {}) +{ + assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* uarray = detail::get_array(u); + PyObject* warray = detail::get_array(w); + + PyObject* plot_args = PyTuple_New(4); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, uarray); + PyTuple_SetItem(plot_args, 3, warray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_quiver, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool stem(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_stem, plot_args); + + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool semilogx(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool semilogy(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool loglog(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool errorbar(const std::vector &x, const std::vector &y, const std::vector &yerr, const std::map &keywords = {}) +{ + assert(x.size() == y.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* yerrarray = detail::get_array(yerr); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyDict_SetItemString(kwargs, "yerr", yerrarray); + + PyObject *plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + + if (res) + Py_DECREF(res); + else + throw std::runtime_error("Call to errorbar() failed."); + + return res; +} + +template +bool named_plot(const std::string& name, const std::vector& y, const std::string& format = "") +{ + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(2); + + PyTuple_SetItem(plot_args, 0, yarray); + PyTuple_SetItem(plot_args, 1, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool plot(const std::vector& y, const std::string& format = "") +{ + std::vector x(y.size()); + for(size_t i=0; i +bool plot(const std::vector& y, const std::map& keywords) +{ + std::vector x(y.size()); + for(size_t i=0; i +bool stem(const std::vector& y, const std::string& format = "") +{ + std::vector x(y.size()); + for (size_t i = 0; i < x.size(); ++i) x.at(i) = i; + return stem(x, y, format); +} + +template +void text(Numeric x, Numeric y, const std::string& s = "") +{ + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(y)); + PyTuple_SetItem(args, 2, PyString_FromString(s.c_str())); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_text, args); + if(!res) throw std::runtime_error("Call to text() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void colorbar(PyObject* mappable = NULL, const std::map& keywords = {}) +{ + if (mappable == NULL) + throw std::runtime_error("Must call colorbar with PyObject* returned from an image, contour, surface, etc."); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, mappable); + + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second)); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_colorbar, args, kwargs); + if(!res) throw std::runtime_error("Call to colorbar() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + + +inline long figure(long number = -1) +{ + PyObject *res; + if (number == -1) + res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); + else { + assert(number > 0); + + // Make sure interpreter is initialised + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(number)); + res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, args); + Py_DECREF(args); + } + + if(!res) throw std::runtime_error("Call to figure() failed."); + + PyObject* num = PyObject_GetAttrString(res, "number"); + if (!num) throw std::runtime_error("Could not get number attribute of figure object"); + const long figureNumber = PyLong_AsLong(num); + + Py_DECREF(num); + Py_DECREF(res); + + return figureNumber; +} + +inline bool fignum_exists(long number) +{ + // Make sure interpreter is initialised + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(number)); + PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, args); + if(!res) throw std::runtime_error("Call to fignum_exists() failed."); + + bool ret = PyObject_IsTrue(res); + Py_DECREF(res); + Py_DECREF(args); + + return ret; +} + +inline void figure_size(size_t w, size_t h) +{ + // Make sure interpreter is initialised + detail::_interpreter::get(); + + const size_t dpi = 100; + PyObject* size = PyTuple_New(2); + PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi)); + PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "figsize", size); + PyDict_SetItemString(kwargs, "dpi", PyLong_FromSize_t(dpi)); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple, kwargs); + + Py_DECREF(kwargs); + + if(!res) throw std::runtime_error("Call to figure_size() failed."); + Py_DECREF(res); +} + +inline void legend() +{ + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple); + if(!res) throw std::runtime_error("Call to legend() failed."); + + Py_DECREF(res); +} + +template +void ylim(Numeric left, Numeric right) +{ + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +template +void xlim(Numeric left, Numeric right) +{ + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + + +inline double* xlim() +{ + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(res); + return arr; +} + + +inline double* ylim() +{ + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(res); + return arr; +} + +template +inline void xticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) +{ + assert(labels.size() == 0 || ticks.size() == labels.size()); + + // using numpy array + PyObject* ticksarray = detail::get_array(ticks); + + PyObject* args; + if(labels.size() == 0) { + // construct positional args + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, ticksarray); + } else { + // make tuple of tick labels + PyObject* labelstuple = PyTuple_New(labels.size()); + for (size_t i = 0; i < labels.size(); i++) + PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); + + // construct positional args + args = PyTuple_New(2); + PyTuple_SetItem(args, 0, ticksarray); + PyTuple_SetItem(args, 1, labelstuple); + } + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xticks, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(!res) throw std::runtime_error("Call to xticks() failed"); + + Py_DECREF(res); +} + +template +inline void xticks(const std::vector &ticks, const std::map& keywords) +{ + xticks(ticks, {}, keywords); +} + +template +inline void yticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) +{ + assert(labels.size() == 0 || ticks.size() == labels.size()); + + // using numpy array + PyObject* ticksarray = detail::get_array(ticks); + + PyObject* args; + if(labels.size() == 0) { + // construct positional args + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, ticksarray); + } else { + // make tuple of tick labels + PyObject* labelstuple = PyTuple_New(labels.size()); + for (size_t i = 0; i < labels.size(); i++) + PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); + + // construct positional args + args = PyTuple_New(2); + PyTuple_SetItem(args, 0, ticksarray); + PyTuple_SetItem(args, 1, labelstuple); + } + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_yticks, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(!res) throw std::runtime_error("Call to yticks() failed"); + + Py_DECREF(res); +} + +template +inline void yticks(const std::vector &ticks, const std::map& keywords) +{ + yticks(ticks, {}, keywords); +} + +inline void tick_params(const std::map& keywords, const std::string axis = "both") +{ + // construct positional args + PyObject* args; + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyString_FromString(axis.c_str())); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_tick_params, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if (!res) throw std::runtime_error("Call to tick_params() failed"); + + Py_DECREF(res); +} + +inline void subplot(long nrows, long ncols, long plot_number) +{ + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args); + if(!res) throw std::runtime_error("Call to subplot() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void subplot2grid(long nrows, long ncols, long rowid=0, long colid=0, long rowspan=1, long colspan=1) +{ + PyObject* shape = PyTuple_New(2); + PyTuple_SetItem(shape, 0, PyLong_FromLong(nrows)); + PyTuple_SetItem(shape, 1, PyLong_FromLong(ncols)); + + PyObject* loc = PyTuple_New(2); + PyTuple_SetItem(loc, 0, PyLong_FromLong(rowid)); + PyTuple_SetItem(loc, 1, PyLong_FromLong(colid)); + + PyObject* args = PyTuple_New(4); + PyTuple_SetItem(args, 0, shape); + PyTuple_SetItem(args, 1, loc); + PyTuple_SetItem(args, 2, PyLong_FromLong(rowspan)); + PyTuple_SetItem(args, 3, PyLong_FromLong(colspan)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot2grid, args); + if(!res) throw std::runtime_error("Call to subplot2grid() failed."); + + Py_DECREF(shape); + Py_DECREF(loc); + Py_DECREF(args); + Py_DECREF(res); +} + +inline void title(const std::string &titlestr, const std::map &keywords = {}) +{ + PyObject* pytitlestr = PyString_FromString(titlestr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pytitlestr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_title, args, kwargs); + if(!res) throw std::runtime_error("Call to title() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void suptitle(const std::string &suptitlestr, const std::map &keywords = {}) +{ + PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pysuptitlestr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_suptitle, args, kwargs); + if(!res) throw std::runtime_error("Call to suptitle() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void axis(const std::string &axisstr) +{ + PyObject* str = PyString_FromString(axisstr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args); + if(!res) throw std::runtime_error("Call to title() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void axvline(double x, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) +{ + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(ymin)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymax)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvline, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); +} + +inline void xlabel(const std::string &str, const std::map &keywords = {}) +{ + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xlabel, args, kwargs); + if(!res) throw std::runtime_error("Call to xlabel() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void ylabel(const std::string &str, const std::map& keywords = {}) +{ + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_ylabel, args, kwargs); + if(!res) throw std::runtime_error("Call to ylabel() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void set_zlabel(const std::string &str, const std::map& keywords = {}) { + // Same as with plot_surface: We lazily load the modules here the first time + // this function is called because I'm not sure that we can assume "matplotlib + // installed" implies "mpl_toolkits installed" on all platforms, and we don't + // want to require it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject *ax = + PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, + detail::_interpreter::get().s_python_empty_tuple); + if (!ax) throw std::runtime_error("Call to gca() failed."); + Py_INCREF(ax); + + PyObject *zlabel = PyObject_GetAttrString(ax, "set_zlabel"); + if (!zlabel) throw std::runtime_error("Attribute set_zlabel not found."); + Py_INCREF(zlabel); + + PyObject *res = PyObject_Call(zlabel, args, kwargs); + if (!res) throw std::runtime_error("Call to set_zlabel() failed."); + Py_DECREF(zlabel); + + Py_DECREF(ax); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} + +inline void grid(bool flag) +{ + PyObject* pyflag = flag ? Py_True : Py_False; + Py_INCREF(pyflag); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyflag); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args); + if(!res) throw std::runtime_error("Call to grid() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void show(const bool block = true) +{ + PyObject* res; + if(block) + { + res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_show, + detail::_interpreter::get().s_python_empty_tuple); + } + else + { + PyObject *kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "block", Py_False); + res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs); + Py_DECREF(kwargs); + } + + + if (!res) throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); +} + +inline void close() +{ + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_close, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to close() failed."); + + Py_DECREF(res); +} + +inline void xkcd() { + PyObject* res; + PyObject *kwargs = PyDict_New(); + + res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd, + detail::_interpreter::get().s_python_empty_tuple, kwargs); + + Py_DECREF(kwargs); + + if (!res) + throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); +} + +inline void draw() +{ + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_draw, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to draw() failed."); + + Py_DECREF(res); +} + +template +inline void pause(Numeric interval) +{ + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args); + if(!res) throw std::runtime_error("Call to pause() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void save(const std::string& filename) +{ + PyObject* pyfilename = PyString_FromString(filename.c_str()); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyfilename); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_save, args); + if (!res) throw std::runtime_error("Call to save() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void clf() { + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_clf, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to clf() failed."); + + Py_DECREF(res); +} + + inline void ion() { + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_ion, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to ion() failed."); + + Py_DECREF(res); +} + +inline std::vector> ginput(const int numClicks = 1, const std::map& keywords = {}) +{ + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(numClicks)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_ginput, args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(args); + if (!res) throw std::runtime_error("Call to ginput() failed."); + + const size_t len = PyList_Size(res); + std::vector> out; + out.reserve(len); + for (size_t i = 0; i < len; i++) { + PyObject *current = PyList_GetItem(res, i); + std::array position; + position[0] = PyFloat_AsDouble(PyTuple_GetItem(current, 0)); + position[1] = PyFloat_AsDouble(PyTuple_GetItem(current, 1)); + out.push_back(position); + } + Py_DECREF(res); + + return out; +} + +// Actually, is there any reason not to call this automatically for every plot? +inline void tight_layout() { + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_tight_layout, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to tight_layout() failed."); + + Py_DECREF(res); +} + +// Support for variadic plot() and initializer lists: + +namespace detail { + +template +using is_function = typename std::is_function>>::type; + +template +struct is_callable_impl; + +template +struct is_callable_impl +{ + typedef is_function type; +}; // a non-object is callable iff it is a function + +template +struct is_callable_impl +{ + struct Fallback { void operator()(); }; + struct Derived : T, Fallback { }; + + template struct Check; + + template + static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match + + template + static std::false_type test( Check* ); + +public: + typedef decltype(test(nullptr)) type; + typedef decltype(&Fallback::operator()) dtype; + static constexpr bool value = type::value; +}; // an object is callable iff it defines operator() + +template +struct is_callable +{ + // dispatch to is_callable_impl or is_callable_impl depending on whether T is of class type or not + typedef typename is_callable_impl::value, T>::type type; +}; + +template +struct plot_impl { }; + +template<> +struct plot_impl +{ + template + bool operator()(const IterableX& x, const IterableY& y, const std::string& format) + { + // 2-phase lookup for distance, begin, end + using std::distance; + using std::begin; + using std::end; + + auto xs = distance(begin(x), end(x)); + auto ys = distance(begin(y), end(y)); + assert(xs == ys && "x and y data must have the same number of elements!"); + + PyObject* xlist = PyList_New(xs); + PyObject* ylist = PyList_New(ys); + PyObject* pystring = PyString_FromString(format.c_str()); + + auto itx = begin(x), ity = begin(y); + for(size_t i = 0; i < xs; ++i) { + PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++)); + PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++)); + } + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xlist); + PyTuple_SetItem(plot_args, 1, ylist); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } +}; + +template<> +struct plot_impl +{ + template + bool operator()(const Iterable& ticks, const Callable& f, const std::string& format) + { + if(begin(ticks) == end(ticks)) return true; + + // We could use additional meta-programming to deduce the correct element type of y, + // but all values have to be convertible to double anyways + std::vector y; + for(auto x : ticks) y.push_back(f(x)); + return plot_impl()(ticks,y,format); + } +}; + +} // end namespace detail + +// recursion stop for the above +template +bool plot() { return true; } + +template +bool plot(const A& a, const B& b, const std::string& format, Args... args) +{ + return detail::plot_impl::type>()(a,b,format) && plot(args...); +} + +/* + * This group of plot() functions is needed to support initializer lists, i.e. calling + * plot( {1,2,3,4} ) + */ +inline bool plot(const std::vector& x, const std::vector& y, const std::string& format = "") { + return plot(x,y,format); +} + +inline bool plot(const std::vector& y, const std::string& format = "") { + return plot(y,format); +} + +inline bool plot(const std::vector& x, const std::vector& y, const std::map& keywords) { + return plot(x,y,keywords); +} + +/* + * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting + */ +class Plot +{ +public: + // default initialization with plot label, some data and format + template + Plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { + + assert(x.size() == y.size()); + + PyObject* kwargs = PyDict_New(); + if(name != "") + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + + if(res) + { + line= PyList_GetItem(res, 0); + + if(line) + set_data_fct = PyObject_GetAttrString(line,"set_data"); + else + Py_DECREF(line); + Py_DECREF(res); + } + } + + // shorter initialization with name or format only + // basically calls line, = plot([], []) + Plot(const std::string& name = "", const std::string& format = "") + : Plot(name, std::vector(), std::vector(), format) {} + + template + bool update(const std::vector& x, const std::vector& y) { + assert(x.size() == y.size()); + if(set_data_fct) + { + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject* res = PyObject_CallObject(set_data_fct, plot_args); + if (res) Py_DECREF(res); + return res; + } + return false; + } + + // clears the plot but keep it available + bool clear() { + return update(std::vector(), std::vector()); + } + + // definitely remove this line + void remove() { + if(line) + { + auto remove_fct = PyObject_GetAttrString(line,"remove"); + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(remove_fct, args); + if (res) Py_DECREF(res); + } + decref(); + } + + ~Plot() { + decref(); + } +private: + + void decref() { + if(line) + Py_DECREF(line); + if(set_data_fct) + Py_DECREF(set_data_fct); + } + + + PyObject* line = nullptr; + PyObject* set_data_fct = nullptr; +}; + +} // end namespace matplotlibcpp diff --git a/Avoidance_Gaussian_Potential_Field/petcat-obstacle-avoidance- b/Avoidance_Gaussian_Potential_Field/petcat-obstacle-avoidance- new file mode 160000 index 0000000..6404eb7 --- /dev/null +++ b/Avoidance_Gaussian_Potential_Field/petcat-obstacle-avoidance- @@ -0,0 +1 @@ +Subproject commit 6404eb768a1b1b1293514cf4549bd11fdb7f5e33 diff --git a/Avoidance_Gaussian_Potential_Field/random_explorer.cpp b/Avoidance_Gaussian_Potential_Field/random_explorer.cpp new file mode 100644 index 0000000..ab8fdc8 --- /dev/null +++ b/Avoidance_Gaussian_Potential_Field/random_explorer.cpp @@ -0,0 +1,56 @@ +#include +#include +#include +#include +#include +#include +#include +#include "GaussianPF_PathPlanning.cpp" + +using namespace std; + +vector lidar_data(360); +const float MAX_VEL = 2; +const float MAX_OMEGA = 0.2; + +float get_steering_msg(vector lidar_dat, float goal_angle){ + // std::reverse(lidar_dat.begin(), lidar_dat.end()); + vector ranges; + ranges.insert(ranges.end(), lidar_dat.begin()+180,lidar_dat.end()); + ranges.insert(ranges.end(), lidar_dat.begin(), lidar_dat.begin()+180); + assert(ranges.size() ==360); + + float h_theta = get_header_rad(ranges, goal_angle, false); + cout << h_theta << endl; + return -1*mapx(0, h_theta, MAX_OMEGA); +} + +void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan){ // Returns the lidar data + lidar_data = scan->ranges; + assert(lidar_data.size() == 360); // Ensuring that the lidar data has been successfully recvd. Replace 360 with 360/(Angular least count for lidar) + cout << "Lidar data recvd:\n"; +} + +int main(int argc, char** argv){ + ros::init(argc, argv, "lidar_subscriber"); + ros::NodeHandle sub_node; + + ros::Subscriber lid_sub = sub_node.subscribe("/husky/laser/scan", 1000, lidarCallback); + + ros::NodeHandle pub_node; + + ros::Publisher lid_pub = pub_node.advertise("/husky_velocity_controller/cmd_vel", 100); + + ros::Rate rate(10); // sets the loop to publish at a rate of 10Hz + float goal_angle = 0; + while(ros::ok()){ + geometry_msgs::Twist msg; + msg.linear.x = 0.5; + msg.angular.z = get_steering_msg(lidar_data, goal_angle); + lid_pub.publish(msg); + ros::spinOnce(); + rate.sleep(); + } + + return 0; +} \ No newline at end of file