Merge pull request #7 from arturmiller/init_sim

Init sim

CMakeLists.txt

@@ -1,19 +1,51 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(alpyca)
+
+find_package(PythonLibs REQUIRED)
+
 find_package(catkin REQUIRED COMPONENTS
   rospy
   message_generation 
-  std_msgs)
+  std_msgs
+  gazebo_ros
+  roscpp
+  pybind11_catkin)
+
 catkin_python_setup()
 
+add_service_files(DIRECTORY srv FILES ExampleSrv.srv)
+generate_messages(DEPENDENCIES std_msgs)
+
+catkin_package(
+    LIBRARIES
+    CATKIN_DEPENDS pybind11_catkin
+)
+
+find_package(gazebo REQUIRED)
+find_package(PythonLibs REQUIRED)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+
+link_directories(${GAZEBO_LIBRARY_DIRS})
+include_directories(include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS})
+
+catkin_package(
+  CATKIN_DEPENDS
+    gazebo_ros
+  LIBRARIES ${PROJECT_NAME}  
+)
+
+pybind_add_module(py_sensor MODULE src/alpyca/sim/py_sensor.cpp)
+pybind_add_module(py_contact_sensor MODULE src/alpyca/sim/py_contact_sensor.cpp)
+
+add_library(plugin_runner SHARED src/alpyca/sim/plugin_runner.cpp)
+target_link_libraries(plugin_runner ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} ${PYTHON_LIBRARIES})
+
+
 catkin_install_python(PROGRAMS
     src/alpyca/__init__.py
     src/alpyca/comm/__init__.py
     src/alpyca/comm/communication.py
     src/alpyca/comm/example.py
 DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/aplyca)
 
-add_service_files(DIRECTORY srv FILES ExampleSrv.srv)
-
-generate_messages(DEPENDENCIES std_msgs)
-

include/alpyca/sim/contact_sensor_wrapper.h

@@ -0,0 +1,115 @@
+/*
+ Wrapper for Gazebo Contact and Contacts. "Repeated" proto message are converted to vector.	
+ */
+
+#ifndef _CONTACT_SENSOR_WRAPPER_H_
+#define _CONTACT_SENSOR_WRAPPER_H_
+
+
+#include <functional>
+#include <iostream>
+#include <vector>
+#include <gazebo/gazebo.hh>
+#include <gazebo/sensors/sensors.hh>
+#include <pybind11/pybind11.h>
+#include "alpyca/sim/sensor_wrapper.h"
+
+
+namespace py = pybind11;
+
+namespace gazebo
+{
+    class ContactWrapper
+    {
+    public:
+        ContactWrapper(msgs::Contact _contact) : contact(_contact)
+        {
+        }
+
+        std::vector<double> depth()
+        {
+            std::vector<double> dep = {};
+            for (unsigned int i = 0; i < contact.position_size(); ++i)
+            {
+                dep.push_back(contact.depth(i));
+            }
+
+            return dep;
+        }
+
+        const::std::string & collision1()
+        {
+            return contact.collision1();
+        }
+
+        const::std::string & collision2()
+        {
+            return contact.collision2();
+        }
+
+        std::vector<gazebo::msgs::Vector3d> position()
+        {
+            std::vector<gazebo::msgs::Vector3d> pos = {};
+            for (unsigned int i = 0; i < contact.position_size(); ++i)
+            {
+                pos.push_back(contact.position(i));
+            }
+
+            return pos;
+        }
+
+        std::vector<gazebo::msgs::Vector3d> normal()
+        {
+            std::vector<gazebo::msgs::Vector3d> norm = {};
+            for (unsigned int i = 0; i < contact.position_size(); ++i)
+            {
+                norm.push_back(contact.normal(i));
+            }
+
+            return norm;
+        }
+
+    private:
+        msgs::Contact contact;
+    };
+
+    class ContactsWrapper
+    {
+    public:
+        ContactsWrapper(msgs::Contacts _contacts) : contacts(_contacts)
+        {
+        }
+
+        ContactWrapper contact(int index)
+        {
+            if(index >= contacts.contact_size())
+            {
+                throw py::index_error();
+            }
+
+            return contacts.contact(index);           
+        }
+
+    private:
+        msgs::Contacts contacts;
+    };
+
+    class ContactSensorWrapper : public SensorWrapper
+    {
+    public:
+        ContactSensorWrapper(sensors::SensorPtr _sensor) : SensorWrapper(_sensor)
+        {
+            parentSensor = std::dynamic_pointer_cast<sensors::ContactSensor>(_sensor);
+        }
+
+        ContactsWrapper Contacts()
+        {
+            return ContactsWrapper(parentSensor->Contacts());
+        }
+
+    private:
+        sensors::ContactSensorPtr parentSensor;
+    };
+}
+
+#endif //_CONTACT_SENSOR_WRAPPER_H_

include/alpyca/sim/plugin_runner.h

@@ -0,0 +1,38 @@
+#ifndef _PLUGIN_RUNNER_
+#define _PLUGIN_RUNNER_
+
+#include <string>
+#include <gazebo/gazebo.hh>
+#include <gazebo/sensors/sensors.hh>
+#include <pybind11/pybind11.h>
+#include <pybind11/embed.h>
+#include "alpyca/sim/contact_sensor_wrapper.h"
+
+
+namespace py = pybind11;
+
+namespace gazebo
+{
+
+  class PluginRunner : public SensorPlugin
+  {
+    public: PluginRunner();
+    public: virtual ~PluginRunner();
+
+    public: virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);
+
+    public: void OnUpdate();
+
+    private:
+        py::object plugin;
+        py::object py_sensor_class;
+        py::object plugin_class;
+        py::object load_func;
+        py::module py_sensor_module;
+        py::module custom_plugin_module;
+        ContactSensorWrapper *sensor_wrapper;
+        py::scoped_interpreter* guard;
+  };
+}
+
+#endif //_PLUGIN_RUNNER_

include/alpyca/sim/sensor_wrapper.h

@@ -0,0 +1,54 @@
+/*
+ Wrapper for Gazebo Sensor.	
+ */
+
+
+#ifndef _SENSOR_WRAPPER_
+#define _SENSOR_WRAPPER_
+
+#include <functional>
+#include <iostream>
+#include <gazebo/gazebo.hh>
+#include <gazebo/sensors/sensors.hh>
+#include <pybind11/pybind11.h>
+
+
+namespace py = pybind11;
+
+namespace gazebo
+{
+    class SensorWrapper{
+    public:
+        SensorWrapper(sensors::SensorPtr _sensor) : sensor(_sensor)
+        {
+        }
+
+        void SetActive(const bool value)
+        {
+            sensor->SetActive(value);
+        }
+
+        void ConnectUpdated(std::function< void()> &_subscriber)
+        {
+            subscriber = _subscriber;
+            updateConnection = sensor->ConnectUpdated(std::bind(&SensorWrapper::OnUpdate, this));
+        }
+
+        void OnUpdate()
+        {
+		    // The gil has to be acquired by this thread to make the Python function not block.
+            py::gil_scoped_release release;
+            {
+                py::gil_scoped_acquire acquire;
+                subscriber();
+            }
+        }
+
+    private:
+        sensors::SensorPtr sensor;
+        event::ConnectionPtr updateConnection;
+        std::function< void()> subscriber;
+    };
+}
+
+#endif //_SENSOR_WRAPPER_

package.xml

@@ -5,17 +5,23 @@
   <description>The main idea of Alpyca is to make robot development with ROS as easy as possible.</description>
 
   <maintainer email="boehle@gmail.com">Sebastian Boehle</maintainer>
-  <author email="boehle@gmail.com">Artur Miller</author>
+  <author email="amiller89@gmx.de">Artur Miller</author>
   <author email="boehle@gmail.com">Sebastian Boehle</author>
   <license>MIT</license>
 
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>pybind11_catkin</build_depend>
+  <build_depend>gazebo_ros</build_depend>
+  <build_depend>roscpp</build_depend>
 
   <run_depend>message_runtime</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>roslaunch</run_depend>
   <run_depend>std_msgs</run_depend>
-
+  <run_depend>pybind11_catkin</run_depend>
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>roscpp</run_depend>
+
 </package>

src/alpyca/sim/README.md

@@ -0,0 +1,52 @@
+# alpyca_sim
+
+**Warning: This project is WIP and does not deliver interfaces for all kinds of plugins yet!**  
+
+Gazebo is a powerful robot simulator. However, there is a steep learning curve to overcome. Even after you know what you are doing, 
+writing plugins keeps being a hard task. Error messages are often cryptic, debugging is hard and you have to wait for your code to compile. 
+It would be much nicer to write plugins in Python. This is what alpyca/sim is for.  
+
+## Example usage
+You can write a contact sensor plugin, exactly as in the [Gazebo tutorials](http://gazebosim.org/tutorials?tut=contact_sensor), but completely in Python. An example can be found in [contact_plugin.py](contact_plugin.py).
+
+```python
+class ContactPlugin:
+    """ A Gazebo ContactPlugin written completely in Python """
+
+    def __init__(self):
+        self.sensor = None
+
+    def Load(self, sensor):
+        """ Load the sensor plugin
+        
+        Parameters
+        ----------
+        sensor : PySensor
+            The sensor that loaded this plugin
+        """
+
+        self.sensor = sensor
+
+        sensor.ConnectUpdated(self.OnUpdate)
+        sensor.SetActive(True)
+
+    def OnUpdate(self):
+        """ Everytime the sensor is updated, this function is called.
+		It receives the sensor's update signal.
+        """
+
+        for contact in self.sensor.Contacts():
+            print("Collision between[{}] and [{}]".format(contact.collision1, contact.collision2))
+
+            for (position, normal, depth) in zip(contact.position, contact.normal, contact.depth):
+                print("Position: {} {} {}".format(position.x, position.y, position.z))
+                print("Normal: {} {} {}".format(normal.x, normal.y, normal.z))
+                print("Depth: {}".format(depth))
+
+```
+
+Run this plugin with:
+```bash
+cd <alpyca_path>/worlds
+gazebo contact.world
+```

src/alpyca/sim/contact_plugin.py

@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+
+
+class ContactPlugin:
+    """ A Gazebo ContactPlugin written completely in Python """
+
+    def __init__(self):
+        self.sensor = None
+
+    def Load(self, sensor):
+        """ Load the sensor plugin
+        
+        Parameters
+        ----------
+        sensor : PySensor
+            The sensor that loaded this plugin
+        """
+
+        self.sensor = sensor
+
+        sensor.ConnectUpdated(self.OnUpdate)
+        sensor.SetActive(True)
+
+    def OnUpdate(self):
+        """ Everytime the sensor is updated, this function is called.
+		It receives the sensor's update signal.
+        """
+
+        for contact in self.sensor.Contacts():
+            print("Collision between[{}] and [{}]".format(contact.collision1, contact.collision2))
+
+            for (position, normal, depth) in zip(contact.position, contact.normal, contact.depth):
+                print("Position: {} {} {}".format(position.x, position.y, position.z))
+                print("Normal: {} {} {}".format(normal.x, normal.y, normal.z))
+                print("Depth: {}".format(depth))