Merge pull request #221 from b-it-bots/devel

Pre-RoboCup 2020 season version

.github/CODEOWNERS

@@ -1,7 +1,7 @@
 # Default owners for everything in the repo.
 # Unless a later match takes precedence, they will be requested for review
 # when someone opens a pull request.
-* @argenos @alex-mitrevski @PatrickNa
+* @argenos @alex-mitrevski @PatrickNa @b-it-bots/pullpanda-junior @b-it-bots/pullpanda-senior
 
 mdr_perception @minhnh
 mdr_navigation @argenos

README.md

@@ -1,4 +1,6 @@
 [![Build Status](https://travis-ci.org/b-it-bots/mas_domestic_robotics.svg?branch=kinetic)](https://travis-ci.org/b-it-bots/mas_domestic_robotics)
+![GitHub tag (latest SemVer)](https://img.shields.io/github/tag/b-it-bots/mas_domestic_robotics.svg?label=version)
+![GitHub](https://img.shields.io/github/license/b-it-bots/mas_domestic_robotics.svg)
 
 # mas_domestic_robotics
 

mdr_perception/mdr_head_controller/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_head_controller)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    roslint
+    rospy
+)
+
+catkin_python_setup()
+roslint_python()
+
+catkin_package(
+  CATKIN_DEPENDS
+    std_msgs
+)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(roslaunch REQUIRED)
+  roslaunch_add_file_check(ros/launch)
+endif()
+
+install(PROGRAMS
+  ros/scripts/head_controller
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

mdr_perception/mdr_head_controller/README.md

@@ -0,0 +1,40 @@
+# mdr_head_controller
+
+A package for functionalities related to controlling a robot's head.
+
+## Existing Functionalities
+
+### head_controller_base
+
+A base implementation of a high-level head controller for performing basic head motion commands, such as tilting the head up or down.
+
+The base class defines the following methods for controlling a head:
+* `look_up`: Moves the robot's head up. Returns a Boolean indicating whether the operation was successful.
+* `look_down`: Moves the robot's head down. Returns a Boolean indicating whether the operation was successful.
+* `turn_left`: Turns the robot's head to the left. Returns a Boolean indicating whether the operation was successful.
+* `turn_right`: Turns the robot's head to the right. Returns a Boolean indicating whether the operation was successful.
+
+Robot-specific implementations need to override all methods.
+
+A script that starts a `head_controller` node is also included in the package as an example, but a robot-specific implementation should start its own node.
+
+## Directory structure
+
+```
+mdr_head_controller
+|    CMakeLists.txt
+|    package.xml
+|    setup.py
+|    README.md
+|____ros
+     |____launch
+     |    |_____head_controller.launch
+     |    |
+     |    scripts
+     |    |_____head_controller
+     |    |
+     |____src
+          |____mdr_head_controller
+               |    __init__.py
+               |____head_controller_base.py
+```

mdr_perception/mdr_head_controller/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package>
+  <name>mdr_head_controller</name>
+  <version>1.0.0</version>
+  <description>Package for functionalities related to controlling a robot's head</description>
+
+  <license>GPLv3</license>
+
+  <author email="[email protected]">Alex Mitrevski</author>
+  <maintainer email="[email protected]">MAS robotics</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>roslint</build_depend>
+
+  <run_depend>std_msgs</run_depend>
+</package>

mdr_perception/mdr_head_controller/ros/launch/head_controller.launch

@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<launch>
+    <node pkg="mdr_head_controller" type="head_controller" name="mdr_head_controller" output="screen">
+    </node>
+</launch>

mdr_perception/mdr_head_controller/ros/scripts/head_controller

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+import rospy
+
+from mdr_head_controller.head_controller_base import HeadControllerBase
+
+if __name__ == '__main__':
+    rospy.init_node('mdr_head_controller')
+    try:
+        HeadControllerBase()
+    except rospy.ROSInterruptException:
+        pass

mdr_perception/mdr_head_controller/ros/src/mdr_head_controller/head_controller_base.py

@@ -0,0 +1,15 @@
+class HeadControllerBase(object):
+    def __init__(self):
+        self.actions = ['look_up', 'look_down', 'turn_left', 'turn_right']
+
+    def look_up(self):
+        raise NotImplementedError('look_up not implemented')
+
+    def look_down(self):
+        raise NotImplementedError('look_down not implemented')
+
+    def turn_left(self):
+        raise NotImplementedError('turn_left not implemented')
+
+    def turn_right(self):
+        raise NotImplementedError('turn_right not implemented')

mdr_perception/mdr_head_controller/setup.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['mdr_head_controller'],
+    package_dir={'mdr_head_controller': 'ros/src/mdr_head_controller'}
+)
+
+setup(**d)

mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_handle_open_action)
+
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  roslint
+  actionlib
+  actionlib_msgs
+  genmsg
+  message_generation
+  geometry_msgs
+)
+
+roslint_python()
+catkin_python_setup()
+
+add_action_files(DIRECTORY ros/action
+  FILES
+  HandleOpen.action
+)
+
+generate_messages(
+  DEPENDENCIES
+  actionlib_msgs
+  geometry_msgs
+)
+
+catkin_package(
+ CATKIN_DEPENDS
+   rospy
+   actionlib
+   actionlib_msgs
+   message_runtime
+   geometry_msgs
+)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+install(PROGRAMS
+  ros/scripts/handle_open_action
+  ros/scripts/handle_open_action_client_test
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts
+)

mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_handle_open_action/README.md

@@ -0,0 +1,56 @@
+# mdr_handle_open_action
+
+An action for manipulating handles.
+
+## Action definition
+
+### Goal:
+
+* ``string handle_type``: Type of the handle (currently unused)
+* ``geometry_msgs/PoseStamped handle_pose``: Pose of the handle
+
+### Result:
+
+* ``bool success``
+
+### Feedback:
+
+* ``string current_state``
+* ``string message``
+
+## Launch file parameters
+
+### Action server
+* ``gripper_controller_pkg_name``: The name of a package that implements functionalities for controlling a robot's gripper (default: 'mdr_gripper_controller')
+* ``move_arm_server``: Name of the move_arm action server (default: 'move_arm_server')
+* ``move_base_server``: Name of the move_base action server (default: 'move_base_server')
+* ``move_forward_server``: Name of the move_forward action server (default: 'move_forward_server')
+* ``force_sensor_topic``: Name of topic for wrist force sensor measurements (default: 'force_sensor_topic')
+* ``pregrasp_config_name``: Name of the pregrasp configuration (default: 'pregrasp_config_name')
+* ``final_config_name``: Name of the final configuration (default: 'final_config_name')
+* ``handle_open_dmp``: Path to a YAML file containing the weights of a dynamic motion primitive used for opening handles (default: '')
+* ``dmp_tau``: The value of the temporal dynamic motion primitive parameter (default: 30)
+
+## Dependencies
+
+* ``numpy``
+* ``scipy``
+* ``cv2``
+* ``cv_bridge``
+* ``pyftsm``
+* ``tf``
+* ``actionlib``
+* ``geometry_msgs``
+* ``mas_execution``
+* ``mdr_rosplan_interface``
+* ``mdr_move_arm_action``
+* ``mdr_move_base_action``
+* ``mdr_move_forward_action``
+
+## Learning data
+
+The package additionally includes some data (under `learning_data`) that was collected in an experiment in which the robot was learning the best grasping position of a handle in our lab while standing at a predefined position in front of our handle. The robot was learning this by experience, namely by sampling grasping poses and labelling those based on the execution success; given those data, a success distribution was learned.
+
+There are two sets of poses there:
+* `sample_grasp_poses_uniform`: Poses sampled from a uniform distribution
+* `sample_grasp_poses_learned`: Poses sampled from the learned success distribution