End-of-2020 version

.bandit

@@ -0,0 +1,4 @@
+skips: 
+    - 'B403' # `import_pickle` (Consider possible security implications associated with these modules.)
+    - 'B301' # `pickle` (Pickle and modules that wrap it can be unsafe when used to deserialize untrusted data, possible security issue.)
+    - 'B311' # `random` (Standard pseudo-random generators are not suitable for security/cryptographic purposes.)

.pylintrc

@@ -0,0 +1,3 @@
+disable=
+    no-self-use,
+    bare-except

.travis.yml

@@ -11,6 +11,7 @@ env:
     - CONTAINER_RELEASE_IMAGE: bitbots/bitbots-domestic:$TRAVIS_BRANCH
 
 before_install:
+  - sudo pip install --upgrade pip
   - sudo rm /usr/local/bin/docker-compose
   - sudo curl -L https://github.com/docker/compose/releases/download/1.22.0/docker-compose-$(uname -s)-$(uname -m) -o /usr/local/bin/docker-compose
   - sudo chmod +x /usr/local/bin/docker-compose

README.md

@@ -55,6 +55,8 @@ All packages in our domestic code base start with the `mdr_` suffix; this stands
 
 ## Getting started
 
+### Development setup
+
 b-it-bots members can use [these instructions](https://github.com/b-it-bots/dev-env#setup) to setup a complete development environment for all our robots.
 
 For external users, the following instructions should get you a working system:
@@ -66,20 +68,29 @@ For external users, the following instructions should get you a working system:
   wstool init src
   wstool merge -t src https://raw.github.com/b-it-bots/mas_domestic_robotics/devel/mas-domestic.rosinstall
   ```
+  **Note:** If you want to set up a development environment, replace `mas-domestic.rosinstall` with `mas-domestic-devel.rosinstall` in the above command.
+
 2. Get the code and dependencies
 
   ```
     wstool update -t src
     rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y
   ```
+  **Note:** If setting up on the robot, skip the `rosdep install` command.
+
 3. Build the code
 
   ```
   catkin build
   ```
+  Make sure that you have sourced `/opt/ros/kinetic/setup.bash` before building the packages.
 
 If you encounter any problems, please check the list of [issues](https://github.com/b-it-bots/mas_domestic_robotics/issues) and open a new one if you don't see a discussion of the problem there.
 
+### Tutorials
+
+A selection of tutorials that explain various architectural decisions and describe some of the commonly used components can be found in the [mas_tutorials](https://github.com/b-it-bots/mas_tutorials) repository.
+
 ## Docker images
 
 If you want to test locally without installing our software and its dependencies, you need to install [docker](https://docs.docker.com/install/linux/docker-ce/ubuntu/) and [docker-compose](https://docs.docker.com/compose/install/).

mas-domestic-devel.rosinstall

@@ -0,0 +1,98 @@
+- git:
+    local-name: mas_common_robotics
+    uri: https://github.com/b-it-bots/mas_common_robotics.git
+    version: kinetic
+
+- git:
+    local-name: mas_domestic_robotics
+    uri: https://github.com/b-it-bots/mas_domestic_robotics.git
+    version: devel
+
+- git:
+    local-name: mas_navigation_tools
+    uri: https://github.com/b-it-bots/mas_navigation_tools.git
+    version: devel
+
+- git:
+    local-name: mas_perception_libs
+    uri: https://github.com/b-it-bots/mas_perception_libs.git
+    version: devel
+
+- git:
+    local-name: mas_perception_msgs
+    uri: https://github.com/b-it-bots/mas_perception_msgs.git
+    version: devel
+
+- git:
+    local-name: mas_execution_manager
+    uri: https://github.com/b-it-bots/mas_execution_manager.git
+    version: devel
+
+- git:
+    local-name: mas_knowledge_base
+    uri: https://github.com/b-it-bots/mas_knowledge_base.git
+    version: devel
+
+- git:
+    local-name: action-execution
+    uri: https://github.com/b-it-bots/action-execution.git
+    version: devel
+
+- git:
+    local-name: ftsm
+    uri: https://github.com/b-it-bots/ftsm.git
+    version: master
+- git:
+    local-name: topological_map
+    uri: https://github.com/b-it-bots/topological_map.git
+    version: master
+
+- git:
+    local-name: zbar_ros
+    uri: https://github.com/b-it-bots/zbar_ros.git
+    version: kinetic
+
+- git:
+    local-name: dmp
+    uri: https://github.com/b-it-bots/dmp.git
+    version: master
+
+- git:
+    local-name: ROSPlan
+    uri: https://github.com/b-it-bots/ROSPlan.git
+    version: master
+
+- git:
+    local-name: mongodb_store
+    uri: https://github.com/b-it-bots/mongodb_store.git
+    version: kinetic-devel
+
+- git:
+    local-name: occupancy_grid_utils
+    uri: https://github.com/b-it-bots/occupancy_grid_utils.git
+    version: indigo-devel
+
+- git:
+    local-name: orocos_kinematics_dynamics
+    uri: https://github.com/b-it-bots/orocos_kinematics_dynamics.git
+    version: master
+
+- git:
+    local-name: kdl_parser
+    uri: https://github.com/ros/kdl_parser.git
+    version: kinetic-devel
+
+- git:
+    local-name: demonstrated_trajectory_recorder
+    uri: https://github.com/b-it-bots/demonstrated_trajectory_recorder.git
+    version: master
+
+- git:
+    local-name: ros_dmp
+    uri: https://github.com/b-it-bots/ros_dmp.git
+    version: master
+
+- git:
+    local-name: mas_tools
+    uri: https://github.com/b-it-bots/mas_tools.git
+    version: devel

mas-domestic.rosinstall

@@ -59,7 +59,7 @@
 
 - git:
     local-name: ROSPlan
-    uri: https://github.com/KCL-Planning/ROSPlan.git
+    uri: https://github.com/b-it-bots/ROSPlan.git
     version: master
 
 - git:
@@ -86,3 +86,8 @@
     local-name: ros_dmp
     uri: https://github.com/b-it-bots/ros_dmp.git
     version: master
+
+- git:
+    local-name: mas_tools
+    uri: https://github.com/b-it-bots/mas_tools.git
+    version: master

mdr_environments/brsu-c069/navigation_goals.yaml

@@ -12,3 +12,13 @@ observation_pose: [-0.386269, -0.149052, -0.013196]
 trash_can: [-1.991786, -3.080515, -1.644647]
 shelf_cupboard: [4.372, -3.300, 3.113]
 trash_bin1: [6.55, -3.75, -0.00277]
+
+monitoring_pose: [-1.249, -2.924, -2.888]
+spot_1: [-0.229, 0.517, -2.784]
+spot_2: [-0.263, 0.756, 0.548]
+spot_3: [0.601, -2.074, 0.823]
+spot_4: [4.238, -1.575, 1.468]
+spot_5: [6.388, -0.584, 1.506]
+spot_6: [6.846, -0.826, -1.132]
+spot_7: [8.274, -3.791, 0.778]
+disinfection_table: [7.095, -3.350, 3.132]

mdr_environments/wrs20-arena/map.yaml

@@ -0,0 +1,33 @@
+# Copyright (c) 2019 TOYOTA MOTOR CORPORATION
+# All rights reserved.
+
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+
+#  * Redistributions of source code must retain the above copyright notice,
+#  this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above copyright
+#  notice, this list of conditions and the following disclaimer in the
+#  documentation and/or other materials provided with the distribution.
+#  * Neither the name of Toyota Motor Corporation nor the names of its
+#  contributors may be used to endorse or promote products derived from
+#  this software without specific prior written permission.
+
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+image: map.pgm
+resolution: 0.050000
+origin: [-5.000000, -5.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

mdr_environments/wrs20-arena/navigation_goals.yaml

@@ -0,0 +1,15 @@
+# pose_name: [x,y,theta]
+
+table_inspection_area1: [0.816, 0.685, 2.235]
+table_inspection_area2: [0.816, 0.685, 1.637]
+table_inspection_area3: [0.816, 0.685, 0.942]
+
+long_table_b: [1.240, 0.978, 1.693]
+tall_table: [0.012, 0.981, 1.554]
+long_table_a_boxes: [1.009, 0.063, -1.544]
+long_table_a_trays: [1.721, 0.187, -1.544]
+bin_a: [2.400, 0.047, -1.544]
+bin_b: [2.748, 0.047, -1.544]
+
+second_room_corridor: [2.535, 1.585, 1.571]
+second_room_cupboard: [2.199, 3.839, 1.501]

mdr_manipulation/mdr_gripper_controller/ros/src/mdr_gripper_controller/gripper_controller_base.py

@@ -2,17 +2,33 @@
 
 class GripperControllerBase(object):
     def open(self):
-        rospy.loginfo('[OPEN_GRIPPER] Ignoring request')
+        rospy.loginfo('[open] Ignoring request')
         raise NotImplementedError()
 
     def close(self):
-        rospy.loginfo('[CLOSE_GRIPPER] Ignoring request')
+        rospy.loginfo('[close] Ignoring request')
+        raise NotImplementedError()
+
+    def init_impact_detection_z(self):
+        rospy.loginfo('[init_impact_detection_z] Ignoring request')
+        raise NotImplementedError()
+
+    def detect_impact_z(self):
+        rospy.loginfo('[detect_impact_z] Ignoring request')
         raise NotImplementedError()
 
     def init_grasp_verification(self):
-        rospy.loginfo('[INIT_GRASP_VERIFICATION] Ignoring request')
+        rospy.loginfo('[init_grasp_verification] Ignoring request')
         raise NotImplementedError()
 
     def verify_grasp(self):
-        rospy.loginfo('[VERIFY_GRASP] Ignoring request')
+        rospy.loginfo('[verify_grasp] Ignoring request')
+        raise NotImplementedError()
+
+    def get_opening_angle(self):
+        rospy.loginfo('[get_opening_angle] Ignoring request')
+        raise NotImplementedError()
+
+    def get_gripper_pose(self, frame_name):
+        rospy.loginfo('[get_gripper_pose] Ignoring request')
         raise NotImplementedError()

mdr_msgs/mdr_manipulation_msgs/CMakeLists.txt

@@ -3,20 +3,26 @@ project(mdr_manipulation_msgs)
 
 find_package(catkin REQUIRED COMPONENTS
   message_generation
+  actionlib_msgs
   geometry_msgs
+  mas_perception_msgs
 )
 
 add_service_files(FILES
   GetBottleState.srv
   Grasp.srv
+  UpdatePlanningScene.srv
 )
 
 generate_messages(DEPENDENCIES
+  actionlib_msgs
   geometry_msgs
+  mas_perception_msgs
 )
 
 catkin_package(
   CATKIN_DEPENDS
     geometry_msgs
     message_runtime
+    mas_perception_msgs
 )

mdr_msgs/mdr_manipulation_msgs/package.xml

@@ -11,9 +11,12 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>message_generation</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>mas_perception_msgs</build_depend>
 
   <run_depend>message_runtime</run_depend>
   <run_depend>geometry_msgs</run_depend>
+  <run_depend>mas_perception_msgs</run_depend>
 
 </package>

mdr_msgs/mdr_manipulation_msgs/srv/UpdatePlanningScene.srv

@@ -0,0 +1,8 @@
+string ADD=add
+string REMOVE=remove
+
+mas_perception_msgs/ObjectList objects
+string operation
+
+---
+bool success

mdr_perception/mdr_cloud_object_detection/CMakeLists.txt

@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mdr_cloud_object_detection)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS
+  actionlib_msgs
+  actionlib
+  dynamic_reconfigure
+  tf2_geometry_msgs
+  pcl_ros
+  roscpp
+  mas_navigation_tools
+  mas_perception_msgs
+  mas_perception_libs
+)
+
+find_package(PCL 1.7 REQUIRED)
+find_package(Boost REQUIRED COMPONENTS python)
+
+generate_dynamic_reconfigure_options(
+  ros/config/ObjectDetection.cfg
+)
+
+catkin_package(
+  INCLUDE_DIRS
+    ros/include
+  LIBRARIES
+    ${PROJECT_NAME}
+  CATKIN_DEPENDS
+    dynamic_reconfigure
+    mas_navigation_tools
+)
+
+###########
+## Build ##
+###########
+include_directories(
+  ros/include
+  ${catkin_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+)
+
+###################################
+# Node for object detection
+add_executable(cloud_object_detection
+  ros/src/CropBoxVoxelFilter.cpp
+  ros/src/CloudObjectDetection.cpp
+  ros/src/main.cpp
+)
+
+add_dependencies(cloud_object_detection ${${PROJECT_NAME}_EXPORTED_TARGETS})
+
+target_link_libraries(cloud_object_detection
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+install(TARGETS cloud_object_detection
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)