Tutorial for humanoid path planner platform.

.gitmodules

+[submodule "cmake"]
+	path = cmake
+	url = git://github.com/jrl-umi3218/jrl-cmakemodules.git

CMakeLists.txt

+#
+# Copyright (c) 2014 CNRS
+# Authors: Florent Lamiraux
+#
+#
+# This file is part of hpp_tutorial
+# hpp_tutorial is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation, either version
+# 3 of the License, or (at your option) any later version.
+#
+# hpp_tutorial is distributed in the hope that it will be
+# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# hpp_tutorial  If not, see
+# <http://www.gnu.org/licenses/>.
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+SET(CXX_DISABLE_WERROR TRUE)
+INCLUDE(cmake/base.cmake)
+INCLUDE(cmake/python.cmake)
+
+SET(PROJECT_NAME hpp_tutorial)
+SET(PROJECT_DESCRIPTION
+  "Tutorial for humanoid path planner platform."
+)
+SET(PROJECT_URL "")
+
+FINDPYTHON()
+
+SETUP_PROJECT()
+
+# Catkin stuff
+FIND_PACKAGE(catkin REQUIRED COMPONENTS
+  hpp_ros)
+CATKIN_PACKAGE()
+
+# Activate hpp-util logging if requested
+SET (HPP_DEBUG FALSE CACHE BOOL "trigger hpp-util debug output")
+IF (HPP_DEBUG)
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_DEBUG")
+ENDIF()
+
+ADD_REQUIRED_DEPENDENCY("hpp-corbaserver >= 3")
+ADD_REQUIRED_DEPENDENCY("hpp_ros")
+
+SET(CATKIN_PACKAGE_SHARE_DESTINATION
+  ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME})
+
+install(FILES launch/tutorial.launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  )
+install(FILES urdf/pr2.urdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
+  )
+install(FILES srdf/pr2.srdf 
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
+  )
+install(FILES rviz/config.rviz
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz
+  )
+install (FILES
+  src/hpp/corbaserver/pr2/robot.py
+  src/hpp/corbaserver/pr2/__init__.py
+  DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/pr2)
+
+SETUP_PROJECT_FINALIZE()

README.md

+Tutorial for humanoid path planner platform.

cmake @ 36780c79

+Subproject commit 36780c793d4b4064a691f59a262c5dad4c1e21ef

launch/tutorial.launch

+<launch>
+  <param name="kitchen_description"
+    textfile="$(find iai_maps)/room/kitchen_area.urdf"/>
+  <param name="robot_description"
+	 textfile="$(find hpp_tutorial)/urdf/pr2.urdf"/>
+  <node name="robot_state_publisher"
+	pkg="robot_state_publisher"
+	type="state_publisher"
+	respawn="true">
+    <param name="tf_prefix" value=""/>
+  </node>
+  <node pkg="tf" type="static_transform_publisher"
+	name="kitchen_link_broadcaster"
+	args="0 0 0 0 0 0 /map /iai_kitchen/kitchen_link 200">
+  </node>
+
+  <node pkg="robot_state_chain_publisher" type="state_chain_publisher"
+    name="kitchen_state_publisher" output="screen">
+    <param name="tf_prefix" value="/iai_kitchen"/>
+    <param name="publish_frequency" value="2"/>
+    <remap from="robot_description" to="kitchen_description" />
+  </node>
+  <node name="rviz" pkg="rviz" type="rviz" respawn="false"
+	args="-d $(find hpp_tutorial)/rviz/config.rviz">
+  </node>
+</launch>

package.xml

+<package>
+  <name>hpp_tutorial</name>
+  <version>0.1.0</version>
+  <description>
+    Tutorial for Humanoid path planner platform
+  </description>
+  <author>Florent Lamiraux</author>
+  <maintainer email="hpp@laas.fr">Florent Lamiraux</maintainer>
+  <license>LGPL v2</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>hpp_ros</build_depend>
+  <build_depend>hpp-corbaserver</build_depend>
+</package>

script/tutorial.py

+from hpp.corbaserver.pr2 import Robot
+from hpp_ros import ScenePublisher
+
+robot = Robot ('pr2')
+r = ScenePublisher (robot)
+q = robot.getCurrentConfig ()
+r (q)
+
+res = robot.distancesToCollision ()
+x = zip (*res)
+y = filter (lambda t:t [0] <= 0.01, x)
+y = filter (lambda t:t[1].find ('caster')!=-1 or t[2].find('caster')!=-1, x)
+
+with file ("./srdf/collision", "w") as f:
+    for l in y:
+        f.write ('  <disable_collisions link1="%s" link2="%s"/>\n'%l[1:3])

src/hpp/corbaserver/pr2/__init__.py

+#!/usr/bin/env python
+# Copyright (c) 2014 CNRS
+# Author: Florent Lamiraux
+#
+# This file is part of hpp-corbaserver.
+# hpp-corbaserver is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation, either version
+# 3 of the License, or (at your option) any later version.
+#
+# hpp-corbaserver is distributed in the hope that it will be
+# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# hpp-corbaserver.  If not, see
+# <http://www.gnu.org/licenses/>.
+
+from robot import Robot

src/hpp/corbaserver/pr2/robot.py

+#!/usr/bin/env python
+# Copyright (c) 2014 CNRS
+# Author: Florent Lamiraux
+#
+# This file is part of hpp-corbaserver.
+# hpp-corbaserver is free software: you can redistribute it
+# and/or modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation, either version
+# 3 of the License, or (at your option) any later version.
+#
+# hpp-corbaserver is distributed in the hope that it will be
+# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# hpp-corbaserver.  If not, see
+# <http://www.gnu.org/licenses/>.
+
+from hpp.corbaserver.robot import Robot as Parent
+
+class Robot (Parent):
+    packageName = "hpp_ros"
+    urdfName = "pr2"
+    urdfSuffix = ""
+    srdfSuffix = ""
+
+    def __init__ (self, robotName):
+        Parent.__init__ (self, robotName, "planar")
+        self.tf_root = "base_footprint"