commit 298b01361b83b56baa5e0438401471dcdb46ad38 Author: Konstantinos Chatzilygeroudis Date: Sat May 10 00:46:24 2014 +0300 0.0.1 Beta Version - Initial Release diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..75a798d --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +nao_dcm_driver-build +CMakeLists.txt.user \ No newline at end of file diff --git a/LICENSE.txt b/LICENSE.txt new file mode 100644 index 0000000..b7f61ff --- /dev/null +++ b/LICENSE.txt @@ -0,0 +1,19 @@ +Copyright (c) 2014, Konstantinos Chatzilygeroudis +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. 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. + +3. Neither the name of the copyright holder 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 HOLDER 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. \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..9aeae6b --- /dev/null +++ b/README.md @@ -0,0 +1,97 @@ +#nao_dcm + +##ROS Stack for Aldebaran's Nao Humanoid + + - Goal is to **connect to the machine of Nao** and not the API provided by Aldebaran. + - Ιt makes use only of the **DCM and Memory Proxies**. + - Supports **V4.0 robots (H21 and H25 Body Types)**. + - **Written purely in C++** and uses the latest *C++ SDK (1.14.5)*. + + +Version +---- + +0.0.1 (Beta) + +Requirements +----------- + +nao_dcm requires several packages to be installed in order to work properly: + +* [ROS] - ROS >= **Hydro** (should also work on the forthcoming *Indigo*) +* [NaoQi C++ SDK] - **Version 1.14.5** +* [ROS MoveIt!] - Used for motion planning +* [ROS Control] - **Version >=0.6.0** +* [Webots for Nao] - The best simulator so far [optional] +* [Gazebo] - Work in progress but with satisfactory results (**Version >= 2.2.2 alongside gazebo-ros-pkgs >= 2.3.4**) [optional] +* [Nao Robot] - A real working Nao is the best "simulator" you'll ever get!! **Version >= V4.0 and flashed OpenNao OS >= 1.14.5** + +Basic Usage +-------------- + +###Bringup nao_dcm driver (remotely) +```sh +roslaunch nao_dcm_bringup nao_dcm_{BodyType}_remote.launch +``` + +{BodyType} is either **H21** or **H25**. + +This will connect to Nao Robot, start the cameras and open up a simple dashboard for monitoring and basic control over Nao. + +###MoveIt Demo +```sh +roslaunch nao_dcm_moveit_config moveit_planner_{BodyType}.launch +``` + +{BodyType} is either **H21** or **H25**. + +*This requires that you have connected to the Robot/Simulation via nao_dcm driver* and will open up **rviz with the Navigation plugin configured** to give Nao trajectories to execute. + +###Webots Simulation + +Launch your **Webots for Nao Simulator** and then **follow the instructions above to bringup nao_dcm driver** (remotely). *On some versions of Webots, it is required that you move the head before you get camera feedback. Also, changing the camera resolution via dynamic reconfigure will not have any effect as Webots uses fixed resolution.* + +###Gazebo Simulation +```sh +roslaunch nao_dcm_gazebo nao_dcm_gazebo_{BodyType}.launch +``` + +{BodyType} is either **H21** or **H25**. + +**NOTE: **This is work in progress and may not work very well. + +This will **launch Gazebo Simulator** and **trajectory controllers** to simulate the real robot. *This is somewhat equivalent as bringing up nao_dcm_driver (remotely)*. Thus, then for example you can use MoveIt Demo to give trajectories to Nao to execute. + +Notes/Limitations +----------------- +* **Before using the nao_dcm driver on your real Nao** *(because DCM needs calibration but Aldebaran does not provide enough documentation on how to calibrate it)*, it is recommended that you connect to your Nao (e.g. via choreographe/motion proxy) and increase the stiffnesses of the joints. +* Currently **nao_dcm can only be run remotely**, *as Nao Robot only supports ROS Groovy locally* and *nao_dcm requires ros_controls packages* (available only on >= Hydro). Yet, **nao_dcm_robot** folder is made to be run locally, so **when ROS Hydro support for Nao locally is available users can use it directly.** +* Tutorials will become available as soon as possible. +* Integration with [nao_meshes] for better visual feedback is in progress. +* Efforts for making ROS Hydro available locally on Nao, because only this way nao_dcm will get its maximum performance. **Help is welcomed.** +* Although my intension is to provide ROS integration to the machine, basic gait and motion planning schemes are on the way for those that want quick results/feedback. +* *Integration for LED, IR and Audio hardware is not available and is not on my agenda*. So, **feel free to contribute in that direction**. +* **This is only ONE part of my Diploma Thesis** (*"Navigation of Humanoid Robots in Unknown Space With Dynamic Obstacles"*), so many parts of the code are not perfect and well-thought. E.g. *initialize* method of Nao class. So, **Pull-Requests for code improvement are welcomed.** + +Origin of the Name +------------------ + +Since my intention is to use the lowest level of Aldebaran's API possible and its name is DCM, I decided to name the ROS Stack nao_dcm. + +License +---- + +BSD + + +Copyright (c) 2014, **Konstantinos Chatzilygeroudis** + +[ros]: http://www.ros.org +[naoqi c++ sdk]: https://community.aldebaran-robotics.com/doc/1-14/index.html +[webots for nao]: https://community.aldebaran-robotics.com/doc/1-14/software/webots/webots_index.html +[gazebo]: http://gazebosim.org/ +[ros moveit!]: http://moveit.ros.org/ +[nao robot]: http://www.aldebaran.com/en/humanoid-robot/nao-robot +[nao_meshes]: https://github.com/vrabaud/nao_meshes +[ros control]: http://wiki.ros.org/ros_control + \ No newline at end of file diff --git a/nao_dcm/CMakeLists.txt b/nao_dcm/CMakeLists.txt new file mode 100644 index 0000000..2d9976b --- /dev/null +++ b/nao_dcm/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(nao_dcm) +find_package(catkin REQUIRED) +catkin_metapackage() \ No newline at end of file diff --git a/nao_dcm/package.xml b/nao_dcm/package.xml new file mode 100644 index 0000000..214ec23 --- /dev/null +++ b/nao_dcm/package.xml @@ -0,0 +1,44 @@ + + + + nao_dcm + 0.0.1 + nao_dcm metapackage contains packages to integrate into ROS Aldebaran's Nao robot (v4). + This metapackage integrates the underlying machine of Nao and not the API provided by Aldebaran. + + + Konstantinos Chatzilygeroudis + + + BSD + + https://github.com/costashatz/nao_dcm + https://github.com/costashatz/nao_dcm/issues + + Konstantinos Chatzilygeroudis + + catkin + + nao_dcm_description + nao_dcm_driver + nao_dcm_msgs + nao_dcm_rqt_dashboard + nao_dcm_control + nao_dcm_gazebo + nao_dcm_moveit_config + + + + + \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_camera/CMakeLists.txt b/nao_dcm_apps/nao_dcm_camera/CMakeLists.txt new file mode 100644 index 0000000..5f8e11c --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 2.8.3) +project(nao_dcm_camera) + +# Tell CMake where to find "FindNAOqi.cmake" +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + image_transport + camera_info_manager + dynamic_reconfigure + nao_dcm_msgs + sensor_msgs +) + +find_package(NAOqi QUIET COMPONENTS + alcommon + alvision + alproxies + alerror + alvalue +) + +find_package(Boost) + +generate_dynamic_reconfigure_options(cfg/NaoDCMCamera.cfg) + +##Needed for ros packages +catkin_package(CATKIN_DEPENDS roscpp std_msgs nao_dcm_msgs sensor_msgs camera_info_manager dynamic_reconfigure) + +if( NAOqi_FOUND AND Boost_FOUND) + message(STATUS "NAOqi C++ SDK found; C++ nodes will be built") + include_directories( include + ${catkin_INCLUDE_DIRS} + ${NAOqi_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS}) + add_executable(nao_dcm_camera src/nao_camera.cpp) + target_link_libraries(nao_dcm_camera + ${catkin_LIBRARIES} + ${NAOqi_LIBRARIES} + ${Boost_LIBRARIES}) + add_dependencies(nao_dcm_camera ${catkin_EXPORTED_TARGETS}) + +else() + message(STATUS "Cannot find NAOqi C++ sdk; C++ nodes will NOT be built") +endif() + +install(TARGETS nao_dcm_camera + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY share + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/nao_dcm_apps/nao_dcm_camera/cfg/NaoDCMCamera.cfg b/nao_dcm_apps/nao_dcm_camera/cfg/NaoDCMCamera.cfg new file mode 100755 index 0000000..bec38e2 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/cfg/NaoDCMCamera.cfg @@ -0,0 +1,50 @@ +#!/usr/bin/env python + +# Copyright (c) 2014, Konstantinos Chatzilygeroudis +# All rights reserved. + +# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +# 2. 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. + +# 3. Neither the name of the copyright holder 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 HOLDER 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. + +PACKAGE = "nao_dcm_camera" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +# Name, Type, Reconfiguration level, Description, Default, Min, Max + +resolutions = gen.enum([gen.const("160x120", int_t, 0, ""), + gen.const("320x240", int_t, 1, ""), + gen.const("640x480", int_t, 2, ""), + gen.const("1280x960", int_t, 3, "") + ], + "Resolutions for camera.") + +gen.add("top_resolution", int_t, 0, + "Camera Top resolution.", 1, edit_method = resolutions) + +gen.add("bottom_resolution", int_t, 0, + "Camera Bottom resolution.", 1, edit_method = resolutions) + +gen.add("top_frame_rate", double_t, 0, + "Camera Top speed (frames per second).", 30.0, 1., 30.0) + +gen.add("bottom_frame_rate", double_t, 0, + "Camera Bottom speed (frames per second).", 30.0, 1., 30.0) + +#gen.add("top_camera_info_url", str_t, 0, +# "Camera Top calibration file (camera_top_rgb8_[top_resolution]) for this video_mode (uncalibrated if null).", "") + +#gen.add("bottom_camera_info_url", str_t, 0, +# "Camera Bottom calibration file (camera_bottom_rgb8_[top_resolution]) for this video_mode (uncalibrated if null).", "") + + +exit(gen.generate(PACKAGE, "NaoDCMCamera", "NaoDCMCamera")) diff --git a/nao_dcm_apps/nao_dcm_camera/cmake/FindNAOqi.cmake b/nao_dcm_apps/nao_dcm_camera/cmake/FindNAOqi.cmake new file mode 100644 index 0000000..5d03579 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/cmake/FindNAOqi.cmake @@ -0,0 +1,149 @@ +# Copyright (c) 2013, Miguel Sarabia +# Imperial College London +# +# 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 the Imperial College London 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. +# + + +# - Try to find NAOqi +# Once executed this script will define the following: +# NAOqi_FOUND - NAOqi was succesfully found +# NAOqi_INCLUDE_DIRS - NAOqi's include directories +# NAOqi_LIBRARIES - NAOqi's libraries +# NAOqi_DIR - Directory where NAOqi was found +#------------------------------------------------------------------------------ +# Users can set NAOqi_DIR to force CMake to look in a particular location, +# setting the AL_DIR environment variable will have a similar effect. + +cmake_minimum_required(VERSION 2.8.3) + +#These are NAOqi's known components (ie. libraries) +set(NAOqi_COMPONENTS + alaudio + albonjourdiscovery + alextractor + allog + almodelutils + alproject + alresource + altools + alautomatictest + alboxrary + alfile + almathinternal + almotion + alpythonbridge + alserial + altts + albehaviorinfo + alcommon + allauncher + almath + almotionrecorder + alpythontools + alsoap + alvalue + albehavior + alerror + allogremote + almemoryfastaccess + alparammanager + alremotecall + althread + alvision + alproxies) + + +#Set INCLUDE hints +set(NAOqi_INCLUDE_HINTS + "${NAOqi_DIR}/include" + "$ENV{AL_DIR}/include" ) + +# Set LIBRARY hints +set(NAOqi_LIBRARY_HINTS + "${NAOqi_DIR}/lib" + "$ENV{AL_DIR}/lib" ) + +# Find include directories +find_path(NAOqi_INCLUDE_DIR alcommon/alproxy.h HINTS ${NAOqi_INCLUDE_HINTS} ) + +# Verify we know about all the components requested +# and remove those we don't know about +set(NAOqi_FILTERED_COMPONENTS ${NAOqi_FIND_COMPONENTS}) + +if ( NAOqi_FIND_COMPONENTS ) + foreach(comp ${NAOqi_FIND_COMPONENTS}) + list(FIND NAOqi_COMPONENTS ${comp} ${comp}_KNOWN) + if (${comp}_KNOWN EQUAL -1) + list(REMOVE_ITEM NAOqi_FILTERED_COMPONENTS ${comp}) + message(STATUS "Unknown NAOqi component ${comp}") + endif() + endforeach() +endif() + +list(LENGTH NAOqi_FILTERED_COMPONENTS NAOqi_NUMBER_OF_COMPONENTS) +set(NAOqi_FOUND_COMPONENTS TRUE) + +# Look for components (ie. libraries) +if( ${NAOqi_NUMBER_OF_COMPONENTS} ) + foreach(comp ${NAOqi_FILTERED_COMPONENTS}) + #Look for the actual library here + find_library(${comp}_LIBRARY NAMES ${comp} HINTS ${NAOqi_LIBRARY_HINTS}) + if ( ${${comp}_LIBRARY} STREQUAL ${comp}_LIBRARY-NOTFOUND) + message(STATUS "Could not find NAOqi's ${comp}") + set(NAOqi_FOUND_COMPONENTS FALSE) + else() + #If everything went well append this component to list of libraries + list(APPEND NAOqi_LIBRARY ${${comp}_LIBRARY}) + endif() + endforeach() +else() + message(STATUS "No NAOqi components specified") +endif() + + +# Handle the QUIET and REQUIRED arguments +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args( + NAOqi #Package name + DEFAULT_MSG + # Variables required to evaluate as TRUE + NAOqi_LIBRARY + NAOqi_INCLUDE_DIR + NAOqi_FOUND_COMPONENTS) + +# Copy the values of the advanced variables to the user-facing ones +set(NAOqi_LIBRARIES ${NAOqi_LIBRARY} ) +set(NAOqi_INCLUDE_DIRS ${NAOqi_INCLUDE_DIR} ) +set(NAOqi_FOUND ${NAOQI_FOUND}) + +# If NAOqi was found, update NAOqi_DIR to show where it was found +if ( NAOqi_FOUND ) + get_filename_component(NAOqi_NEW_DIR "${NAOqi_INCLUDE_DIRS}/../" ABSOLUTE) +endif() +set(NAOqi_DIR ${NAOqi_NEW_DIR} CACHE FILEPATH "NAOqi root directory" FORCE) + +#Hide these variables +mark_as_advanced(NAOqi_INCLUDE_DIR NAOqi_LIBRARY NAOQI_FOUND) diff --git a/nao_dcm_apps/nao_dcm_camera/include/nao_dcm_camera/nao_camera.h b/nao_dcm_apps/nao_dcm_camera/include/nao_dcm_camera/nao_camera.h new file mode 100644 index 0000000..9119294 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/include/nao_dcm_camera/nao_camera.h @@ -0,0 +1,124 @@ +/**Copyright (c) 2014, Konstantinos Chatzilygeroudis +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. 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. + +3. Neither the name of the copyright holder 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 HOLDER 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. +**/ +#ifndef NAO_CAMERA_H +#define NAO_CAMERA_H + +// Boost Headers +#include +#include +#include + +// NAOqi Headers +#include +#include +#include +#include + +// ROS Headers +#include + +#include +#include +#include + +#include +#include + +#include + +#include +#include + +using std::string; +using std::cerr; +using std::endl; + +namespace AL +{ +class ALBroker; +} + +class NaoCamera : public AL::ALModule +{ +private: + // ROS Standard Variables + ros::NodeHandle node_handle_, top_node_handle_, bottom_node_handle_; + + // ROS Topics/Messages + image_transport::CameraPublisher camera_top_image_pub_, camera_bottom_image_pub_; + sensor_msgs::CameraInfo camera_top_, camera_bottom_; + sensor_msgs::Image camera_top_image_, camera_bottom_image_; + + ros::ServiceServer camera_top_switch_, camera_bottom_switch_; + + boost::shared_ptr camera_top_info_, camera_bottom_info_; + + // ROS Dynamic Reconfigure + dynamic_reconfigure::Server dynamic_req_server_; + nao_dcm_camera::NaoDCMCameraConfig cameras_config_; + volatile bool reconfiguring_; + + // Member Variables + + // Camera Specific + bool camera_top_enabled_, camera_bottom_enabled_, camera_enabled_; + + // Helper + bool is_connected_; + int topic_queue_; + string prefix_; + + // AL Proxies + AL::ALVideoDeviceProxy video_proxy_; +public: + // Constructor/Destructor + NaoCamera(boost::shared_ptr broker, const std::string& name); + ~NaoCamera(); + + bool initialize(); + + // Connect/Disconnet to ALProxies + bool connect(const ros::NodeHandle nh, const ros::NodeHandle top, const ros::NodeHandle bottom); + void disconnect(); + + // Subscribe to ROS Topics/Services + void subscribe(); + + // Parameter Server + void loadParams(); + + // Helper + void brokerDisconnected(const string& event_name, const string &broker_name, const string& subscriber_identifier); + string getURLFromConfiguration(const string& camera, const int& resolution, const int &color = 0); + + // ALVideoDevice Methods + AL::ALValue getImage(const string &cam_module="NaoCameraTop"); + + // General Methods + void loop(const ros::Time& time, const ros::Duration& period); + + bool connected(); + + // ROS Callbacks/Related Methods + + void publishCameras(const ros::Time &ts); + + bool switchCameraTopState(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res); + bool switchCameraBottomState(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res); + + void dynamicReconfigureCb(nao_dcm_camera::NaoDCMCameraConfig &config, uint32_t level); + +}; + +#endif // NAO_H diff --git a/nao_dcm_apps/nao_dcm_camera/package.xml b/nao_dcm_apps/nao_dcm_camera/package.xml new file mode 100644 index 0000000..fbd14a0 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/package.xml @@ -0,0 +1,52 @@ + + + + nao_dcm_camera + 0.0.1 + Package containing the camera driver to connect to Aldebaran's Nao robot's cameras. + + Konstantinos Chatzilygeroudis + + + BSD + + https://github.com/costashatz/nao_dcm + https://github.com/costashatz/nao_dcm/issues + + Konstantinos Chatzilygeroudis + + + catkin + + roscpp + std_msgs + image_transport + camera_info_manager + dynamic_reconfigure + nao_dcm_msgs + sensor_msgs + + + roscpp + std_msgs + image_transport + camera_info_manager + dynamic_reconfigure + nao_dcm_msgs + sensor_msgs + + + + + \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_1280x960.yaml b/nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_1280x960.yaml new file mode 100644 index 0000000..43589ad --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_1280x960.yaml @@ -0,0 +1,20 @@ +image_width: 1280 +image_height: 960 +camera_name: NaoCameraBottom +camera_matrix: + rows: 3 + cols: 3 + data: [1219.448171, 0, 614.481756, 0, 1291.421024, 151.265905, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [0.005928, 0.118999, -0.081849, -0.003667, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] +projection_matrix: + rows: 3 + cols: 4 + data: [1249.13147, 0, 611.294675, 0, 0, 1188.448242, 108.11769, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_160x120.yaml b/nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_160x120.yaml new file mode 100644 index 0000000..50d066a --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_160x120.yaml @@ -0,0 +1,20 @@ +image_width: 160 +image_height: 120 +camera_name: NaoCameraBottom +camera_matrix: + rows: 3 + cols: 3 + data: [155.224589, 0, 80.709808, 0, 158.41574, 25.169391, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.0771, 0.160121, -0.038934, 0.004482, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] +projection_matrix: + rows: 3 + cols: 4 + data: [154.612289, 0, 80.756829, 0, 0, 151.163651, 22.228715, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_320x240.yaml b/nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_320x240.yaml new file mode 100644 index 0000000..aeed437 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_320x240.yaml @@ -0,0 +1,20 @@ +image_width: 320 +image_height: 240 +camera_name: NaoCameraBottom +camera_matrix: + rows: 3 + cols: 3 + data: [321.873789, 0, 157.06588, 0, 328.583553, 51.991062, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.058408, 0.681358, -0.055512, -0.004355, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] +projection_matrix: + rows: 3 + cols: 4 + data: [349.489349, 0, 156.09221, 0, 0, 346.640076, 47.94287, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_640x480.yaml b/nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_640x480.yaml new file mode 100644 index 0000000..d7bc92c --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_640x480.yaml @@ -0,0 +1,20 @@ +image_width: 640 +image_height: 480 +camera_name: NaoCameraBottom +camera_matrix: + rows: 3 + cols: 3 + data: [581.996595, 0, 313.488982, 0, 583.026436, 210.571778, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.035206, 0.08788, 0.002187, -0.001672, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] +projection_matrix: + rows: 3 + cols: 4 + data: [584.987427, 0, 312.169289, 0, 0, 584.710754, 211.522273, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_1280x960.yaml b/nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_1280x960.yaml new file mode 100644 index 0000000..c9ebf5d --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_1280x960.yaml @@ -0,0 +1,20 @@ +image_width: 1280 +image_height: 960 +camera_name: NaoCameraTop +camera_matrix: + rows: 3 + cols: 3 + data: [1248.631984, 0, 559.742668, 0, 1260.921559, 349.814455, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.02015, 0.030736, -0.02731, -0.011039, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] +projection_matrix: + rows: 3 + cols: 4 + data: [1255.832153, 0, 547.89151, 0, 0, 1245.243896, 329.706716, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_160x120.yaml b/nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_160x120.yaml new file mode 100644 index 0000000..9f265df --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_160x120.yaml @@ -0,0 +1,20 @@ +image_width: 160 +image_height: 120 +camera_name: NaoCameraTop +camera_matrix: + rows: 3 + cols: 3 + data: [149.968982, 0, 80.31209, 0, 150.997704, 48.927609, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.082615, 0.173998, -0.021707, -0.006771, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] +projection_matrix: + rows: 3 + cols: 4 + data: [150.092209, 0, 78.890158, 0, 0, 149.09726, 46.450828, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_320x240.yaml b/nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_320x240.yaml new file mode 100644 index 0000000..f4625cd --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_320x240.yaml @@ -0,0 +1,20 @@ +image_width: 320 +image_height: 240 +camera_name: NaoCameraTop +camera_matrix: + rows: 3 + cols: 3 + data: [294.056719, 0, 156.161902, 0, 298.870595, 61.772464, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [0.002381, -0.079306, -0.036802, -0.004818, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] +projection_matrix: + rows: 3 + cols: 4 + data: [294.832886, 0, 154.23345, 0, 0, 282.856323, 54.554838, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_640x480.yaml b/nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_640x480.yaml new file mode 100644 index 0000000..a1005a9 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_640x480.yaml @@ -0,0 +1,20 @@ +image_width: 640 +image_height: 480 +camera_name: NaoCameraTop +camera_matrix: + rows: 3 + cols: 3 + data: [554.903478, 0, 306.432389, 0, 553.554369, 208.613332, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.09743, 0.125595, -0.016384, -0.008456, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] +projection_matrix: + rows: 3 + cols: 4 + data: [550.039795, 0, 300.903728, 0, 0, 544.60376, 200.810548, 0, 0, 0, 1, 0] \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp b/nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp new file mode 100644 index 0000000..8d7cfa0 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp @@ -0,0 +1,444 @@ +/**Copyright (c) 2014, Konstantinos Chatzilygeroudis +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. 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. + +3. Neither the name of the copyright holder 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 HOLDER 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. +**/ +#include +#include "nao_dcm_camera/nao_camera.h" +#include +#include + +using std::vector; + +NaoCamera::NaoCamera(boost::shared_ptr broker, const string &name) + : AL::ALModule(broker,name),is_connected_(false),reconfiguring_(false) +{ + functionName("brokerDisconnected", getName(), "Callback when broker disconnects!"); + BIND_METHOD(NaoCamera::brokerDisconnected); +} + +NaoCamera::~NaoCamera() +{ + if(is_connected_) + disconnect(); +} + +bool NaoCamera::initialize() +{ + dynamic_req_server_.setCallback(boost::bind(&NaoCamera::dynamicReconfigureCb, this, _1, _2)); +} + +bool NaoCamera::connect(const ros::NodeHandle nh, const ros::NodeHandle top, const ros::NodeHandle bottom) +{ + node_handle_ = nh; + top_node_handle_ = top; + bottom_node_handle_ = bottom; + is_connected_ = false; + + // Loada parameter server parameters + loadParams(); + + // If camera not enabled, skip initialization + if(!camera_enabled_) + { + ROS_WARN("Cameras not enabled! Shutting down NaoCamera node.."); + is_connected_ = true; + return true; + } + try + { + video_proxy_ = AL::ALVideoDeviceProxy(getParentBroker()); + + if(camera_top_enabled_) + { + video_proxy_.subscribeCamera("NaoCameraTop", 0, 1, 11, 30); + video_proxy_.setActiveCamera("NaoCameraTop",0); + } + + if(camera_bottom_enabled_) + { + video_proxy_.subscribeCamera("NaoCameraBottom", 1, 1, 11, 30); + video_proxy_.setActiveCamera("NaoCameraBottom",1); + } + } + catch (const AL::ALError& e) + { + ROS_ERROR("Failed to connect to Camera Proxy!\n\tTrace: %s",e.what()); + return false; + } + + // Initialize Cameras Info + camera_top_info_ = boost::shared_ptr + (new camera_info_manager::CameraInfoManager(top_node_handle_)); + camera_top_info_->setCameraName("NaoCameraTop"); + + camera_bottom_info_ = boost::shared_ptr + (new camera_info_manager::CameraInfoManager(bottom_node_handle_)); + camera_bottom_info_->setCameraName("NaoCameraBottom"); + + // Load Default Camera Configurations + cameras_config_.top_frame_rate = video_proxy_.getFrameRate(0); + cameras_config_.top_resolution = video_proxy_.getResolution(0); + + cameras_config_.bottom_frame_rate = video_proxy_.getFrameRate(1); + cameras_config_.top_resolution = video_proxy_.getResolution(1); + + camera_top_info_->loadCameraInfo(getURLFromConfiguration("top",cameras_config_.top_resolution)); + camera_bottom_info_->loadCameraInfo(getURLFromConfiguration("bottom",cameras_config_.bottom_resolution)); + + is_connected_ = true; + subscribe(); + + if(!initialize()) + return false; + ROS_INFO("NaoCamera Module initialized!"); + + return true; +} + +void NaoCamera::disconnect() +{ + if(!is_connected_) + return; + try + { + video_proxy_.unsubscribe("NaoCameraTop"); + video_proxy_.unsubscribe("NaoCameraBottom"); + } + catch (const AL::ALError& e) + { + ROS_ERROR("Failed to unsubscribe from cameras!\n\tTrace: %s",e.what()); + } + is_connected_ = false; +} + +void NaoCamera::loadParams() +{ + node_handle_.param("Nao_UseCamera", camera_enabled_, true); + node_handle_.param("Nao_TopCameraEnabled", camera_top_enabled_, camera_enabled_); + node_handle_.param("Nao_BottomCameraEnabled", camera_bottom_enabled_, camera_enabled_); + + node_handle_.param("Nao_TopicQueue", topic_queue_, 50); + + node_handle_.param("Nao_Prefix", prefix_, string("nao_dcm")); + prefix_ = prefix_+"/"; +} + +void NaoCamera::brokerDisconnected(const string& event_name, const string &broker_name, + const string& subscriber_identifier) +{ + if(broker_name == "Nao Camera Broker") + is_connected_ = false; +} + +string NaoCamera::getURLFromConfiguration(const string &camera, const int &resolution, const int &color) +{ + // Color is not used as we allow only for rgb8 mode of the cameras (we could provide more in the future) + + string res = "640x480"; + switch (resolution) { + case 0: + res = "160x120"; + break; + case 1: + res = "320x240"; + break; + case 2: + res = "640x480"; + break; + case 3: + res = "1280x960"; + break; + default: + res = "640x480"; + break; + } + return "package://nao_dcm_camera/share/camera_"+camera+"_rgb8_"+res+".yaml"; +} + +void NaoCamera::subscribe() +{ + image_transport::ImageTransport it(node_handle_); + + camera_top_image_pub_ = it.advertiseCamera(prefix_+"CameraTop/image_raw", topic_queue_); + + camera_bottom_image_pub_ = it.advertiseCamera(prefix_+"CameraBottom/image_raw", topic_queue_); + + camera_top_switch_ = node_handle_.advertiseService(prefix_+"CameraTop/Enable", &NaoCamera::switchCameraTopState, this); + + camera_bottom_switch_ = node_handle_.advertiseService(prefix_+"CameraBottom/Enable", + &NaoCamera::switchCameraBottomState, this); +} + +AL::ALValue NaoCamera::getImage(const string& cam_module) +{ + AL::ALValue results; + bool err=false; + try + { + results = video_proxy_.getImageRemote(cam_module); + } + catch(const AL::ALError& e) + { + err=true; + ROS_ERROR("Could not get/write image.\n\tTrace: %s",e.what()); + } + if(!err) + { + try + { + video_proxy_.releaseImage(cam_module); + } + catch(const AL::ALError& e) + { + ROS_ERROR("Could not release image.\n\tTrace: %s",e.what()); + } + } + return results; +} + +void NaoCamera::loop(const ros::Time &time, const ros::Duration &period) +{ + if(!camera_enabled_) + { + is_connected_ = false; + return; + } + if(reconfiguring_) + return; + try + { + publishCameras(time); + } + catch(ros::Exception& e) + { + ROS_WARN("NaoCamera Error: %s",e.what()); + } + catch(AL::ALError& e) + { + ROS_ERROR("ALError!\n\tTrace: %s",e.what()); + } +} + +bool NaoCamera::connected() +{ + return is_connected_; +} + +void NaoCamera::publishCameras(const ros::Time &ts) +{ + if(camera_top_enabled_) + { + AL::ALValue im_info = getImage("NaoCameraTop"); + if(im_info.getSize()!=12) + { + return; + } + const unsigned char* im = static_cast(im_info[6].GetBinary()); + if(im==NULL) + return; + + int w, h, nbLayers; + w = (int)im_info[0]; + h = (int)im_info[1]; + nbLayers = (int)im_info[2]; + vector v(im, (im+w*h*nbLayers)); + camera_top_.header.frame_id = "CameraTop"; + camera_top_.header.stamp = ts; + + camera_top_ = camera_top_info_->getCameraInfo(); + camera_top_.header.frame_id = "CameraTop"; + camera_top_.header.stamp = ts; + + camera_top_image_.header.frame_id = "CameraTop"; + camera_top_image_.header.stamp = ts; + + camera_top_image_.data = v; + camera_top_image_.width = w; + camera_top_image_.height = h; + camera_top_image_.step = w*nbLayers; + // TO-DO: Need to find encoding from Nao Image + camera_top_image_.encoding = "rgb8"; + camera_top_image_pub_.publish(camera_top_image_, camera_top_); + } + + if(camera_bottom_enabled_) + { + AL::ALValue im_info = getImage("NaoCameraBottom"); + if(im_info.getSize()!=12) + { + return; + } + const unsigned char* im = static_cast(im_info[6].GetBinary()); + if(im==NULL) + return; + + int w = (int)im_info[0]; + int h = (int)im_info[1]; + int nbLayers = (int)im_info[2]; + vector v = vector (im, (im+w*h*nbLayers)); + + camera_bottom_.header.frame_id = "CameraBottom"; + camera_bottom_.header.stamp = ts; + + camera_bottom_ = camera_bottom_info_->getCameraInfo(); + camera_bottom_.header.frame_id = "CameraBottom"; + camera_bottom_.header.stamp = ts; + + camera_bottom_image_.header.frame_id = "CameraBottom"; + camera_bottom_image_.header.stamp = ts; + + camera_bottom_image_.data = v; + camera_bottom_image_.width = w; + camera_bottom_image_.height = h; + camera_bottom_image_.step = w*nbLayers; + // TO-DO: Need to find encoding from Nao Image + camera_bottom_image_.encoding = "rgb8"; + camera_bottom_image_pub_.publish(camera_bottom_image_, camera_bottom_); + } +} + +bool NaoCamera::switchCameraTopState(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res) +{ + if(!camera_enabled_) + return true; + camera_top_enabled_ = req.enable; + return true; +} + +bool NaoCamera::switchCameraBottomState(nao_dcm_msgs::BoolService::Request &req, + nao_dcm_msgs::BoolService::Response &res) +{ + if(!camera_enabled_) + return true; + camera_bottom_enabled_ = req.enable; + return true; +} + +void NaoCamera::dynamicReconfigureCb(nao_dcm_camera::NaoDCMCameraConfig &config, uint32_t level) +{ + reconfiguring_ = true; + // Change configurations if needed + if(camera_top_enabled_) + { + if(cameras_config_.top_resolution != config.top_resolution) + { + video_proxy_.setResolution("NaoCameraTop", config.top_resolution); + camera_top_info_->loadCameraInfo(getURLFromConfiguration("top", config.top_resolution)); + } + if(cameras_config_.top_frame_rate != config.top_frame_rate) + { + video_proxy_.setFrameRate("NaoCameraTop", config.top_frame_rate); + } + } + + if(camera_bottom_enabled_) + { + if(cameras_config_.bottom_resolution != config.bottom_resolution) + { + video_proxy_.setResolution("NaoCameraBottom", config.bottom_resolution); + camera_bottom_info_->loadCameraInfo(getURLFromConfiguration("bottom",config.bottom_resolution)); + } + if(cameras_config_.bottom_frame_rate != config.bottom_frame_rate) + { + video_proxy_.setFrameRate("NaoCameraBottom", config.bottom_frame_rate); + } + } + + // Store new configurations + cameras_config_ = config; + + reconfiguring_ = false; +} + +int main( int argc, char** argv ) +{ + int pport = 9559; + string pip = "127.0.0.1"; + ros::init(argc, argv, "nao_dcm_camera"); + ros::NodeHandle n; + ros::NodeHandle top("CameraTopNode"), bottom("CameraBottomNode"); + if(!ros::master::check()) + { + cerr<<"Could not contact master!\nQuitting... "< broker; + try + { + broker = AL::ALBroker::createBroker(broker_name,broker_ip,broker_port,pip,pport,0); + } + catch(...) + { + ROS_ERROR("Failed to connect to Broker at %s:%d!",pip.c_str(),pport); + AL::ALBrokerManager::getInstance()->removeBroker(broker); + return -1; + } + + // Deal with ALBrokerManager singleton (add your broker into NAOqi) + AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock()); + AL::ALBrokerManager::getInstance()->addBroker(broker); + + // Now it's time to load your module + boost::shared_ptr nao_cam = AL::ALModule::createModule(broker, "NaoCamera"); + nao_cam->connect(n,top,bottom); + if(!nao_cam->connected()) + { + ROS_ERROR("Could not connect to Nao robot!"); + AL::ALBrokerManager::getInstance()->killAllBroker(); + AL::ALBrokerManager::kill(); + return -1; + } + ros::Rate rate(communication_rate); + // Run the spinner in a separate thread to prevent lockups + ros::AsyncSpinner spinner(1); + spinner.start(); + if(broker->isModulePresent("NaoCamera")) + ROS_INFO("NaoCamera Module loaded succesfully!"); + else + { + ROS_ERROR("NaoCamera Module is not loaded!"); + return -1; + } + while(ros::ok()) + { + nao_cam->loop(ros::Time::now(), rate.expectedCycleTime()); + if(!nao_cam->connected()) + { + ROS_WARN("NaoCamera unexpectedly disconnected!"); + break; + } + rate.sleep(); + } + AL::ALBrokerManager::getInstance()->killAllBroker(); + AL::ALBrokerManager::kill(); + spinner.stop(); + ROS_INFO( "Quitting... " ); + return 0; +} diff --git a/nao_dcm_apps/nao_dcm_gazebo/CMakeLists.txt b/nao_dcm_apps/nao_dcm_gazebo/CMakeLists.txt new file mode 100644 index 0000000..4dfc3c4 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_gazebo/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 2.8.3) +project(nao_dcm_gazebo) + +## Find catkin +find_package(catkin REQUIRED COMPONENTS gazebo_ros) + +##Needed for ros packages +catkin_package() + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY worlds + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/nao_dcm_apps/nao_dcm_gazebo/config/gazebo_ros_control_params.yaml b/nao_dcm_apps/nao_dcm_gazebo/config/gazebo_ros_control_params.yaml new file mode 100644 index 0000000..20a9969 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_gazebo/config/gazebo_ros_control_params.yaml @@ -0,0 +1,52 @@ +nao_dcm/gazebo_ros_control/pid_gains: + HeadYaw: {p: 60.0, i: 80.0, d: 0.3} + + HeadPitch: {p: 60.0, i: 80.0, d: 0.3} + + LShoulderPitch: {p: 60.0, i: 80.0, d: 0.3} + + LElbowYaw: {p: 60.0, i: 80.0, d: 0.3} + + LElbowRoll: {p: 60.0, i: 80.0, d: 0.3} + + LShoulderRoll: {p: 60.0, i: 80.0, d: 0.3} + + LWristYaw: {p: 60.0, i: 80.0, d: 0.3} + + LHand: {p: 60.0, i: 80.0, d: 0.3} + + RShoulderPitch: {p: 60.0, i: 80.0, d: 0.3} + + RElbowYaw: {p: 60.0, i: 80.0, d: 0.3} + + RElbowRoll: {p: 60.0, i: 80.0, d: 0.3} + + RShoulderRoll: {p: 60.0, i: 80.0, d: 0.3} + + RWristYaw: {p: 60.0, i: 80.0, d: 0.3} + + RHand: {p: 60.0, i: 80.0, d: 0.3} + + LHipYawPitch: {p: 60.0, i: 80.0, d: 0.3} + + LHipRoll: {p: 60.0, i: 80.0, d: 0.3} + + LHipPitch: {p: 60.0, i: 80.0, d: 0.3} + + LKneePitch: {p: 60.0, i: 80.0, d: 0.3} + + LAnklePitch: {p: 60.0, i: 80.0, d: 0.3} + + LAnkleRoll: {p: 60.0, i: 80.0, d: 0.3} + + RHipYawPitch: {p: 60.0, i: 80.0, d: 0.3} + + RHipRoll: {p: 60.0, i: 80.0, d: 0.3} + + RHipPitch: {p: 60.0, i: 80.0, d: 0.3} + + RKneePitch: {p: 60.0, i: 80.0, d: 0.3} + + RAnklePitch: {p: 60.0, i: 80.0, d: 0.3} + + RAnkleRoll: {p: 60.0, i: 80.0, d: 0.3} \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H21.launch b/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H21.launch new file mode 100644 index 0000000..2e1a06c --- /dev/null +++ b/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H21.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H25.launch b/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H25.launch new file mode 100644 index 0000000..14dc745 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H25.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_gazebo/package.xml b/nao_dcm_apps/nao_dcm_gazebo/package.xml new file mode 100644 index 0000000..a57a650 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_gazebo/package.xml @@ -0,0 +1,39 @@ + + + + nao_dcm_gazebo + 0.0.1 + Simulate Aldebaran's NAO robot (v4) in Gazebo simulator + + Konstantinos Chatzilygeroudis + + BSD + + https://github.com/costashatz/nao_dcm + https://github.com/costashatz/nao_dcm/issues + + Konstantinos Chatzilygeroudis + + + catkin + + gazebo_ros + + nao_dcm_description + gazebo_ros + + + + + \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_gazebo/worlds/nao_test.world b/nao_dcm_apps/nao_dcm_gazebo/worlds/nao_test.world new file mode 100644 index 0000000..444328b --- /dev/null +++ b/nao_dcm_apps/nao_dcm_gazebo/worlds/nao_test.world @@ -0,0 +1,16 @@ + + + + + model://ground_plane + + + model://sun + + + model://gas_station + gas_station + -2.0 7.0 0 0 0 0 + + + \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/.setup_assistant b/nao_dcm_apps/nao_dcm_moveit_config/.setup_assistant new file mode 100644 index 0000000..4c6b6fb --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/.setup_assistant @@ -0,0 +1,8 @@ +moveit_setup_assistant_config: + URDF: + package: nao_dcm_description + relative_path: urdf/nao_dcm_H25.xacro + SRDF: + relative_path: config/nao_robot_v4_H25.srdf + CONFIG: + generated_timestamp: 1398368537 \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/CMakeLists.txt b/nao_dcm_apps/nao_dcm_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..c838162 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 2.8.3) +project(nao_dcm_moveit_config) + +find_package(catkin REQUIRED) + +catkin_package() + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/controllers_H21.yaml b/nao_dcm_apps/nao_dcm_moveit_config/config/controllers_H21.yaml new file mode 100644 index 0000000..852d779 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/config/controllers_H21.yaml @@ -0,0 +1,53 @@ +controller_manager_ns: '' +controller_list: + - name: nao_dcm/LeftArm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - LShoulderPitch + - LShoulderRoll + - LElbowYaw + - LElbowRoll + - name: nao_dcm/RightArm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - RShoulderPitch + - RShoulderRoll + - RElbowYaw + - RElbowRoll + - name: nao_dcm/Pelvis_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - LHipYawPitch + - name: nao_dcm/LeftLeg_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - LHipRoll + - LHipPitch + - LKneePitch + - LAnklePitch + - LAnkleRoll + - name: nao_dcm/RightLeg_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - RHipRoll + - RHipPitch + - RKneePitch + - RAnklePitch + - RAnkleRoll + - name: nao_dcm/Head_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - HeadYaw + - HeadPitch \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/controllers_H25.yaml b/nao_dcm_apps/nao_dcm_moveit_config/config/controllers_H25.yaml new file mode 100644 index 0000000..14e37ef --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/config/controllers_H25.yaml @@ -0,0 +1,67 @@ +controller_manager_ns: '' +controller_list: + - name: nao_dcm/LeftArm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - LShoulderPitch + - LShoulderRoll + - LElbowYaw + - LElbowRoll + - name: nao_dcm/RightArm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - RShoulderPitch + - RShoulderRoll + - RElbowYaw + - RElbowRoll + - name: nao_dcm/LeftHand_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - LWristYaw + - LHand + - name: nao_dcm/RightHand_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - RWristYaw + - RHand + - name: nao_dcm/Pelvis_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - LHipYawPitch + - name: nao_dcm/LeftLeg_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - LHipRoll + - LHipPitch + - LKneePitch + - LAnklePitch + - LAnkleRoll + - name: nao_dcm/RightLeg_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - RHipRoll + - RHipPitch + - RKneePitch + - RAnklePitch + - RAnkleRoll + - name: nao_dcm/Head_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - HeadYaw + - HeadPitch \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/fake_controllers.yaml b/nao_dcm_apps/nao_dcm_moveit_config/config/fake_controllers.yaml new file mode 100644 index 0000000..d26fef3 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/config/fake_controllers.yaml @@ -0,0 +1,42 @@ +controller_list: + - name: fake_LeftArm_controller + joints: + - LShoulderPitch + - LShoulderRoll + - LElbowYaw + - LElbowRoll + - name: fake_RightArm_controller + joints: + - RShoulderPitch + - RShoulderRoll + - RElbowYaw + - RElbowRoll + - name: fake_Head_controller + joints: + - HeadYaw + - HeadPitch + - name: fake_LeftLeg_controller + joints: + - LHipRoll + - LHipPitch + - LKneePitch + - LAnklePitch + - LAnkleRoll + - name: fake_RightLeg_controller + joints: + - RHipRoll + - RHipPitch + - RKneePitch + - RAnklePitch + - RAnkleRoll + - name: fake_Pelvis_controller + joints: + - LHipYawPitch + - name: fake_RightHand_controller + joints: + - RWristYaw + - RHand + - name: fake_LeftHand_controller + joints: + - LWristYaw + - LHand \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/joint_limits.yaml b/nao_dcm_apps/nao_dcm_moveit_config/config/joint_limits.yaml new file mode 100644 index 0000000..28116f1 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/config/joint_limits.yaml @@ -0,0 +1,126 @@ +joint_limits: + HeadPitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + HeadYaw: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LAnklePitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LAnkleRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LElbowRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LElbowYaw: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LHand: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LHipPitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LHipRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LHipYawPitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LKneePitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LShoulderPitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LShoulderRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LWristYaw: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RAnklePitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RAnkleRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RElbowRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RElbowYaw: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RHand: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RHipPitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RHipRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RKneePitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RShoulderPitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RShoulderRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RWristYaw: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/joint_limits_H21.yaml b/nao_dcm_apps/nao_dcm_moveit_config/config/joint_limits_H21.yaml new file mode 100644 index 0000000..6e07a19 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/config/joint_limits_H21.yaml @@ -0,0 +1,106 @@ +joint_limits: + HeadPitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + HeadYaw: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LAnklePitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LAnkleRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LElbowRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LElbowYaw: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LHipPitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LHipRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LHipYawPitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LKneePitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LShoulderPitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + LShoulderRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RAnklePitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RAnkleRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RElbowRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RElbowYaw: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RHipPitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RHipRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RKneePitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RShoulderPitch: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + RShoulderRoll: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/kinematics.yaml b/nao_dcm_apps/nao_dcm_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..c067867 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/config/kinematics.yaml @@ -0,0 +1,40 @@ +LeftArm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 +RightArm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 +Head: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 +LeftLeg: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 +RightLeg: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 +Pelvis: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 +RightHand: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 +LeftHand: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/kinematics_H21.yaml b/nao_dcm_apps/nao_dcm_moveit_config/config/kinematics_H21.yaml new file mode 100644 index 0000000..d24ec14 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/config/kinematics_H21.yaml @@ -0,0 +1,30 @@ +LeftArm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 +RightArm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 +Head: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 +LeftLeg: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 +RightLeg: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 +Pelvis: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/nao_dcm_moveit_config.rviz b/nao_dcm_apps/nao_dcm_moveit_config/config/nao_dcm_moveit_config.rviz new file mode 100644 index 0000000..9ecf32a --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/config/nao_dcm_moveit_config.rviz @@ -0,0 +1,465 @@ +Panels: + - Class: rviz/Displays + Help Height: 81 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /MotionPlanning1 + - /MotionPlanning1/Planning Request1 + Splitter Ratio: 0.5 + Tree Height: 330 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + Name: MotionPlanning + Planned Path: + Links: + All Links Enabled: true + CameraBottom: + Alpha: 1 + Show Axes: false + Show Trail: false + CameraTop: + Alpha: 1 + Show Axes: false + Show Trail: false + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + LElbow: + Alpha: 1 + Show Axes: false + Show Trail: false + LElbowDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LHip: + Alpha: 1 + Show Axes: false + Show Trail: false + LHipDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LHipYaw: + Alpha: 1 + Show Axes: false + Show Trail: false + LKnee: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LShoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + LShoulderDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RElbow: + Alpha: 1 + Show Axes: false + Show Trail: false + RElbowDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RHip: + Alpha: 1 + Show Axes: false + Show Trail: false + RHipDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RHipYaw: + Alpha: 1 + Show Axes: false + Show Trail: false + RKnee: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RShoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + RShoulderDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + SonarLeft: + Alpha: 1 + Show Axes: false + Show Trail: false + SonarRight: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + gaze: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_ankle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + neck: + Alpha: 1 + Show Axes: false + Show Trail: false + r_ankle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: /move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Frobenius Condition Number: false + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Minimum Singular Value: false + Show Weight Limit: false + TextHeight: 0.08 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: LeftArm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.9 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + CameraBottom: + Alpha: 1 + Show Axes: false + Show Trail: false + CameraTop: + Alpha: 1 + Show Axes: false + Show Trail: false + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + LElbow: + Alpha: 1 + Show Axes: false + Show Trail: false + LElbowDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LHip: + Alpha: 1 + Show Axes: false + Show Trail: false + LHipDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LHipYaw: + Alpha: 1 + Show Axes: false + Show Trail: false + LKnee: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LShoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + LShoulderDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RElbow: + Alpha: 1 + Show Axes: false + Show Trail: false + RElbowDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RHip: + Alpha: 1 + Show Axes: false + Show Trail: false + RHipDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RHipYaw: + Alpha: 1 + Show Axes: false + Show Trail: false + RKnee: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RShoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + RShoulderDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + SonarLeft: + Alpha: 1 + Show Axes: false + Show Trail: false + SonarRight: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + gaze: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_ankle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + l_sole: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + neck: + Alpha: 1 + Show Axes: false + Show Trail: false + r_ankle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper: + Alpha: 1 + Show Axes: false + Show Trail: false + r_sole: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.79867 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.440398 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.240398 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1031 + Hide Left Dock: false + Hide Right Dock: false + Motion Planning: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000027c00000362fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000001dc000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000225000001800000017500ffffff000000010000010f00000362fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004300000362000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000023a00fffffffb0000000800540069006d00650100000000000004500000000000000000000003e90000036200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 25 diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H21.srdf b/nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H21.srdf new file mode 100644 index 0000000..a8cc16b --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H21.srdf @@ -0,0 +1,130 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H25.srdf b/nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H25.srdf new file mode 100644 index 0000000..ee6ad0f --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H25.srdf @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H21.yaml b/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H21.yaml new file mode 100644 index 0000000..c34e518 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H21.yaml @@ -0,0 +1,111 @@ +planner_configs: + SBLkConfigDefault: + type: geometric::SBL + ESTkConfigDefault: + type: geometric::EST + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + BKPIECEkConfigDefault: + type: geometric::BKPIECE + KPIECEkConfigDefault: + type: geometric::KPIECE + RRTkConfigDefault: + type: geometric::RRT + RRTConnectkConfigDefault: + type: geometric::RRTConnect + RRTstarkConfigDefault: + type: geometric::RRTstar + TRRTkConfigDefault: + type: geometric::TRRT + PRMkConfigDefault: + type: geometric::PRM + PRMstarkConfigDefault: + type: geometric::PRMstar +LeftArm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(LShoulderPitch,LShoulderRoll) + longest_valid_segment_fraction: 0.05 +RightArm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(RShoulderPitch,RShoulderRoll) + longest_valid_segment_fraction: 0.05 +Head: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(HeadYaw,HeadPitch) + longest_valid_segment_fraction: 0.05 +LeftLeg: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(LHipRoll,LHipPitch) + longest_valid_segment_fraction: 0.05 +RightLeg: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(RHipRoll,RHipPitch) + longest_valid_segment_fraction: 0.05 +Pelvis: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H25.yaml b/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H25.yaml new file mode 100644 index 0000000..f6600e2 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H25.yaml @@ -0,0 +1,141 @@ +planner_configs: + SBLkConfigDefault: + type: geometric::SBL + ESTkConfigDefault: + type: geometric::EST + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + BKPIECEkConfigDefault: + type: geometric::BKPIECE + KPIECEkConfigDefault: + type: geometric::KPIECE + RRTkConfigDefault: + type: geometric::RRT + RRTConnectkConfigDefault: + type: geometric::RRTConnect + RRTstarkConfigDefault: + type: geometric::RRTstar + TRRTkConfigDefault: + type: geometric::TRRT + PRMkConfigDefault: + type: geometric::PRM + PRMstarkConfigDefault: + type: geometric::PRMstar +LeftArm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(LShoulderPitch,LShoulderRoll) + longest_valid_segment_fraction: 0.05 +RightArm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(RShoulderPitch,RShoulderRoll) + longest_valid_segment_fraction: 0.05 +Head: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(HeadYaw,HeadPitch) + longest_valid_segment_fraction: 0.05 +LeftLeg: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(LHipRoll,LHipPitch) + longest_valid_segment_fraction: 0.05 +RightLeg: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(RHipRoll,RHipPitch) + longest_valid_segment_fraction: 0.05 +Pelvis: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault +RightHand: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(RWristYaw,RHand) + longest_valid_segment_fraction: 0.05 +LeftHand: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(LWristYaw,LHand) + longest_valid_segment_fraction: 0.05 \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/default_warehouse_db.launch b/nao_dcm_apps/nao_dcm_moveit_config/launch/default_warehouse_db.launch new file mode 100644 index 0000000..2f9f4f3 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/demo.launch b/nao_dcm_apps/nao_dcm_moveit_config/launch/demo.launch new file mode 100644 index 0000000..e032751 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/demo.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + [/move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/nao_dcm_apps/nao_dcm_moveit_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..634d969 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/move_group_H21.launch b/nao_dcm_apps/nao_dcm_moveit_config/launch/move_group_H21.launch new file mode 100644 index 0000000..a2fec05 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/move_group_H21.launch @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/move_group_H25.launch b/nao_dcm_apps/nao_dcm_moveit_config/launch/move_group_H25.launch new file mode 100644 index 0000000..c71f44b --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/move_group_H25.launch @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/moveit.rviz b/nao_dcm_apps/nao_dcm_moveit_config/launch/moveit.rviz new file mode 100644 index 0000000..c814852 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/moveit.rviz @@ -0,0 +1,689 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.74256 + Tree Height: 664 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + Name: MotionPlanning + Planned Path: + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: /move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: left_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Scene Robot: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: /odom + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.9965 + Focal Point: + X: 0.113567 + Y: 0.10592 + Z: 2.23518e-07 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.509797 + Target Frame: /base_link + Value: XYOrbit (rviz) + Yaw: 5.65995 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1337 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + Motion Planning: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1828 + X: 459 + Y: -243 diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/moveit_planner_H21.launch b/nao_dcm_apps/nao_dcm_moveit_config/launch/moveit_planner_H21.launch new file mode 100644 index 0000000..78345e2 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/moveit_planner_H21.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/moveit_planner_H25.launch b/nao_dcm_apps/nao_dcm_moveit_config/launch/moveit_planner_H25.launch new file mode 100644 index 0000000..6d4bde6 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/moveit_planner_H25.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/moveit_rviz.launch b/nao_dcm_apps/nao_dcm_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 0000000..8b881d7 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H21_moveit_controller_manager.launch.xml b/nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H21_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..500964d --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H21_moveit_controller_manager.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H21_moveit_sensor_manager.launch.xml b/nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H21_moveit_sensor_manager.launch.xml new file mode 100644 index 0000000..5d02698 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H21_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H25_moveit_controller_manager.launch.xml b/nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H25_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..33bc766 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H25_moveit_controller_manager.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H25_moveit_sensor_manager.launch.xml b/nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H25_moveit_sensor_manager.launch.xml new file mode 100644 index 0000000..5d02698 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H25_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/ompl_planning_pipeline.launch.xml b/nao_dcm_apps/nao_dcm_moveit_config/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 0000000..02f7dcf --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/planning_context_H21.launch b/nao_dcm_apps/nao_dcm_moveit_config/launch/planning_context_H21.launch new file mode 100644 index 0000000..d2706dc --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/planning_context_H21.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/planning_context_H25.launch b/nao_dcm_apps/nao_dcm_moveit_config/launch/planning_context_H25.launch new file mode 100644 index 0000000..75b1e69 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/planning_context_H25.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/planning_pipeline.launch.xml b/nao_dcm_apps/nao_dcm_moveit_config/launch/planning_pipeline.launch.xml new file mode 100644 index 0000000..bcc1aad --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/run_benchmark_ompl.launch b/nao_dcm_apps/nao_dcm_moveit_config/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..5fb40c2 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/sensor_manager.launch.xml b/nao_dcm_apps/nao_dcm_moveit_config/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..73c8743 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/sensor_manager.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/setup_assistant.launch b/nao_dcm_apps/nao_dcm_moveit_config/launch/setup_assistant.launch new file mode 100644 index 0000000..0f6b87c --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/setup_assistant.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/trajectory_execution.launch.xml b/nao_dcm_apps/nao_dcm_moveit_config/launch/trajectory_execution.launch.xml new file mode 100644 index 0000000..d4a0baf --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/warehouse.launch b/nao_dcm_apps/nao_dcm_moveit_config/launch/warehouse.launch new file mode 100644 index 0000000..667925f --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/launch/warehouse_settings.launch.xml b/nao_dcm_apps/nao_dcm_moveit_config/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..d11aaeb --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/nao_dcm_apps/nao_dcm_moveit_config/package.xml b/nao_dcm_apps/nao_dcm_moveit_config/package.xml new file mode 100644 index 0000000..40c5351 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_moveit_config/package.xml @@ -0,0 +1,40 @@ + + + + nao_dcm_moveit_config + 0.0.1 + + Configuration and launch files for using Aldebaran's Nao (v4) with the MoveIt Motion Planning Framework. + + + Konstantinos Chatzilygeroudis + + BSD + + https://github.com/costashatz/nao_dcm + https://github.com/costashatz/nao_dcm/issues + + Konstantinos Chatzilygeroudis + + catkin + + moveit_ros_move_group + moveit_planners_ompl + moveit_ros_visualization + joint_state_publisher + robot_state_publisher + xacro + nao_dcm_description + + diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/CMakeLists.txt b/nao_dcm_apps/nao_dcm_rqt_dashboard/CMakeLists.txt new file mode 100644 index 0000000..c44c8f9 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_rqt_dashboard/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 2.8.3) +project(nao_dcm_rqt_dashboard) + +find_package(catkin REQUIRED) +find_package(Qt4 REQUIRED) + + +catkin_package() +catkin_python_setup() + +install(PROGRAMS scripts/nao_dcm_rqt_dashboard + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(FILES plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/package.xml b/nao_dcm_apps/nao_dcm_rqt_dashboard/package.xml new file mode 100644 index 0000000..c996710 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_rqt_dashboard/package.xml @@ -0,0 +1,41 @@ + + + + nao_dcm_rqt_dashboard + 0.0.1 + + The NaoDCM dashboard is a RQT-based plug-in for visualising data from the Nao and giving easy access + to basic functionalities. + + + Konstantinos Chatzilygeroudis + + BSD + + https://github.com/costashatz/nao_dcm + https://github.com/costashatz/nao_dcm/issues + + Konstantinos Chatzilygeroudis + + catkin + diagnostic_msgs + rospy + rqt_gui + rqt_gui_py + rqt_robot_dashboard + + + + + diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/plugin.xml b/nao_dcm_apps/nao_dcm_rqt_dashboard/plugin.xml new file mode 100644 index 0000000..aded269 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_rqt_dashboard/plugin.xml @@ -0,0 +1,35 @@ + + + + + + A Python GUI plugin for displaying a dashboard that displays and interacts with low level Nao DCM states. + + + + + folder + Plugins related to specific robots. + + + + folder + Plugins related to the Nao robot. + + + task-past-due + A Python GUI plugin for displaying a dashboard that displays and interacts with low level Nao DCM states. + + + diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/scripts/nao_dcm_rqt_dashboard b/nao_dcm_apps/nao_dcm_rqt_dashboard/scripts/nao_dcm_rqt_dashboard new file mode 100755 index 0000000..dcf75a2 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_rqt_dashboard/scripts/nao_dcm_rqt_dashboard @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +import sys +import roslib + +pkg = 'nao_dcm_rqt_dashboard' +roslib.load_manifest(pkg) + +from rqt_gui.main import Main + +main = Main() +sys.exit(main.main(sys.argv, standalone=pkg)) \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/setup.py b/nao_dcm_apps/nao_dcm_rqt_dashboard/setup.py new file mode 100755 index 0000000..21f0cf8 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_rqt_dashboard/setup.py @@ -0,0 +1,9 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup(packages=['nao_dcm_rqt_dashboard'], + package_dir={'': 'src'}) + +setup(**d) diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/__init__.py b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/battery.py b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/battery.py new file mode 100755 index 0000000..78fe9e7 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/battery.py @@ -0,0 +1,44 @@ +# Copyright (c) 2014, Konstantinos Chatzilygeroudis +# All rights reserved. + +# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +# 2. 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. + +# 3. Neither the name of the copyright holder 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 HOLDER 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. + +import roslib;roslib.load_manifest('nao_dcm_rqt_dashboard') +import rospy + +from rqt_robot_dashboard.widgets import BatteryDashWidget + +class NaoDCMBatteryWidget(BatteryDashWidget): + def __init__(self, name='battery'): + icons = [] + charge_icons = [] + icons.append(['ic-battery-0.svg']) + icons.append(['ic-battery-20.svg']) + icons.append(['ic-battery-40.svg']) + icons.append(['ic-battery-60-green.svg']) + icons.append(['ic-battery-80-green.svg']) + icons.append(['ic-battery-100-green.svg']) + charge_icons.append(['ic-battery-charge-0.svg']) + charge_icons.append(['ic-battery-charge-20.svg']) + charge_icons.append(['ic-battery-charge-40.svg']) + charge_icons.append(['ic-battery-charge-60-green.svg']) + charge_icons.append(['ic-battery-charge-80-green.svg']) + charge_icons.append(['ic-battery-charge-100-green.svg']) + super(NaoDCMBatteryWidget, self).__init__(name=name, icons=icons, charge_icons=charge_icons) + + self._pct = 0.0 + self._vlt = 0.0 + + def set_power_state(self, msg): + self._pct = float(msg) + + self.update_perc(self._pct) + self.update_time(self._pct) diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/battery.pyc b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/battery.pyc new file mode 100644 index 0000000..d6c5c9e Binary files /dev/null and b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/battery.pyc differ diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/camera.py b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/camera.py new file mode 100644 index 0000000..b33845f --- /dev/null +++ b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/camera.py @@ -0,0 +1,123 @@ +# Copyright (c) 2014, Konstantinos Chatzilygeroudis +# All rights reserved. + +# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +# 2. 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. + +# 3. Neither the name of the copyright holder 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 HOLDER 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. + +import roslib;roslib.load_manifest('nao_dcm_rqt_dashboard') +import rospy + +from python_qt_binding.QtCore import Signal, QMutex, QMutexLocker +from python_qt_binding.QtGui import QWidget, QPixmap, QImage, QGraphicsView, QGraphicsScene, QVBoxLayout + +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +from rqt_robot_dashboard.widgets import IconToolButton +import numpy + +class CameraViewDashWidget(IconToolButton): + def __init__(self, context, camera_topic='/image_raw', name='camera'): + self._icons = [['bg-grey.svg', 'ol-play-badge.svg']] + super(CameraViewDashWidget, self).__init__("View"+name, icons=self._icons, icon_paths=None) + self.context = context + self.update_state(0) + + self._cam_view = CameraViewWidget(camera_topic,name) + + self._cam_view_shown = False + self.clicked.connect(self._show_cam_view) + self._show_mutex = QMutex() + + def _show_cam_view(self): + with QMutexLocker(self._show_mutex): + try: + if self._cam_view_shown: + self.context.remove_widget(self._cam_view) + self._cam_view_shown = not self._cam_view_shown + else: + self.context.add_widget(self._cam_view) + self._cam_view_shown = not self._cam_view_shown + except Exception: + self._cam_view_shown = not self._cam_view_shown + self._show_cam_view() + + def shutdown_widget(self): + if self._cam_view: + self._cam_view.close() + + +class CameraViewWidget(QWidget): + def __init__(self, camera_topic='/image_raw', name='camera'): + super(CameraViewWidget,self).__init__() + + self._layout = QVBoxLayout() + self.name = name + self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46), (102, 224, 18), (242, 156, 6), (240, 64, 10), (196, 30, 250)] + + + self.setWindowTitle('Camera \''+name+'\' Viewer') + + self._cam_view = CameraView(camera_topic) + + self._layout.addWidget(self._cam_view) + + self.setLayout(self._layout) + + def close(self): + if self._cam_view: + self._cam_view.close() + + +class CameraView(QGraphicsView): + image_changed = Signal() + + def __init__(self, camera_topic='/image_raw'): + super(CameraView,self).__init__() + + self._scene = QGraphicsScene() + self.bridge = CvBridge() + self._map_item = 0 + + self.image_changed.connect(self._update) + + self._sub = rospy.Subscriber(camera_topic,Image,self.callback) + + self.setScene(self._scene) + + def callback(self, msg): + self.w = msg.width + self.h = msg.height + + a = self.bridge.imgmsg_to_cv(msg, "rgb8") + a = numpy.array(a) + image = QImage(a, self.w, self.h, QImage.Format_RGB888) + self._map = image + self._scene.setSceneRect(0,0,self.w,self.h) + self.image_changed.emit() + + def _update(self): + if self._map_item: + self._scene.removeItem(self._map_item) + pixmap = QPixmap.fromImage(self._map) + + self._map_item = self._scene.addPixmap(pixmap) + + self.centerOn(self._map_item) + self.show() + + + def _mirror(self, item): + item.scale(-1, 1) + item.translate(-self.w, 0) + + def close(self): + if self._sub: + self._sub.unregister() + super(CameraView, self).close() \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/camera.pyc b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/camera.pyc new file mode 100644 index 0000000..4134498 Binary files /dev/null and b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/camera.pyc differ diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/nao_dcm_rqt_dashboard.py b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/nao_dcm_rqt_dashboard.py new file mode 100755 index 0000000..84d13c2 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/nao_dcm_rqt_dashboard.py @@ -0,0 +1,81 @@ +# Copyright (c) 2014, Konstantinos Chatzilygeroudis +# All rights reserved. + +# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +# 2. 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. + +# 3. Neither the name of the copyright holder 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 HOLDER 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. + +import roslib;roslib.load_manifest('nao_dcm_rqt_dashboard') +import rospy + +import diagnostic_msgs + +from rqt_robot_dashboard.dashboard import Dashboard +from rqt_robot_dashboard.widgets import MonitorDashWidget, ConsoleDashWidget, MenuDashWidget, IconToolButton, NavViewDashWidget +from QtGui import QMessageBox, QAction +from python_qt_binding.QtCore import QSize + +from .battery import NaoDCMBatteryWidget +from .camera import CameraViewDashWidget +from .stiffness import NaoDCMStiffnessWidget + +import rospkg +import os.path + +rp = rospkg.RosPack() + +class NaoDCMDashboard(Dashboard): + def setup(self, context): + self._dashboard_message = None + self._last_dashboard_message_time = 0.0 + self.name = "NaoDCM Dashboard" + + rospy.sleep(0.5) + + self.motor_bat = NaoDCMBatteryWidget("Battery") + self.console = ConsoleDashWidget(self.context) + self.monitor = MonitorDashWidget(self.context) + self.monitor._show_monitor() + self.nav = NavViewDashWidget(self.context) + self.cam = CameraViewDashWidget(self.context, 'nao_dcm/CameraTop/image_raw', 'CameraTop') + self.cam._show_cam_view() + self.camBot = CameraViewDashWidget(self.context, 'nao_dcm/CameraBottom/image_raw', 'CameraBottom') + self.camBot._show_cam_view() + self.stiff = NaoDCMStiffnessWidget("Stiffness") + + # This is what gets dashboard_callback going eagerly + self._dashboard_agg_sub = rospy.Subscriber('diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray, self.dashboard_callback) + + def get_widgets(self): + return [[self.monitor, self.console],[self.motor_bat],[self.nav],[self.cam, self.camBot],[self.stiff]] + + def dashboard_callback(self, msg): + self._dashboard_message = msg + self._last_dashboard_message_time = rospy.get_time() + + battery_status = {} # Used to store Battery status info + for status in msg.status: + if status.name == "/Nao/Battery/Battery": + for value in status.values: + battery_status[value.key]=value.value + + # If battery diagnostics were found, calculate percentages and stuff + if (battery_status): + self.motor_bat.set_power_state(battery_status['Battery Charge']) + + def shutdown_dashboard(self): + self._dashboard_agg_sub.unregister() + + def save_settings(self, plugin_settings, instance_settings): + self.console.save_settings(plugin_settings, instance_settings) + self.monitor.save_settings(plugin_settings, instance_settings) + + def restore_settings(self, plugin_settings, instance_settings): + self.console.restore_settings(plugin_settings, instance_settings) + self.monitor.restore_settings(plugin_settings, instance_settings) diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/nao_dcm_rqt_dashboard.pyc b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/nao_dcm_rqt_dashboard.pyc new file mode 100644 index 0000000..ed5abb9 Binary files /dev/null and b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/nao_dcm_rqt_dashboard.pyc differ diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/stiffness.py b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/stiffness.py new file mode 100644 index 0000000..5123b12 --- /dev/null +++ b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/stiffness.py @@ -0,0 +1,50 @@ +# Copyright (c) 2014, Konstantinos Chatzilygeroudis +# All rights reserved. + +# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +# 2. 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. + +# 3. Neither the name of the copyright holder 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 HOLDER 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. + +import roslib;roslib.load_manifest('nao_dcm_rqt_dashboard') +import rospy +from nao_dcm_msgs.srv import BoolService, BoolServiceRequest +from std_msgs.msg import Float32 + +from rqt_robot_dashboard.widgets import IconToolButton + +class NaoDCMStiffnessWidget(IconToolButton): + def __init__(self, name='stiffness'): + self.name = name + icons = [] + icons.append(['bg-red.svg','ic-motors.svg']) + icons.append(['bg-green.svg','ic-motors.svg']) + super(NaoDCMStiffnessWidget,self).__init__(name=name,icons=icons) + self.update_state(0) + + self.stiffness = 1.0 + self.clicked.connect(self.changeStiffness) + self._sub = rospy.Subscriber('/nao_dcm/stiffnesses',Float32,self.callback) + + def changeStiffness(self): + stiff = rospy.ServiceProxy('/nao_dcm/Stiffnesses/Enable',BoolService) + req = BoolServiceRequest() + if(self.stiffness==1.0): + req.enable = False + else: + req.enable = True + self.stiffness = 1.0-self.stiffness + stiff(req) + + def callback(self, msg): + self.stiffness = msg.data + + if(msg.data == 1.0): + self.update_state(0) + else: + self.update_state(1) \ No newline at end of file diff --git a/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/stiffness.pyc b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/stiffness.pyc new file mode 100644 index 0000000..298c74c Binary files /dev/null and b/nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/stiffness.pyc differ diff --git a/nao_dcm_common/nao_dcm_control/CMakeLists.txt b/nao_dcm_common/nao_dcm_control/CMakeLists.txt new file mode 100644 index 0000000..c10735a --- /dev/null +++ b/nao_dcm_common/nao_dcm_control/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.3) +project(nao_dcm_control) + +## Find catkin +find_package(catkin REQUIRED) + +##Needed for ros packages +catkin_package() + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/nao_dcm_common/nao_dcm_control/config/nao_position_control.yaml b/nao_dcm_common/nao_dcm_control/config/nao_position_control.yaml new file mode 100644 index 0000000..a764351 --- /dev/null +++ b/nao_dcm_common/nao_dcm_control/config/nao_position_control.yaml @@ -0,0 +1,112 @@ +nao_dcm: + # Publish all joint states ----------------------------------- + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + + # Controllers ------------------------------------------------ + HeadYaw_position_controller: + type: position_controllers/JointPositionController + joint: HeadYaw + + HeadPitch_position_controller: + type: position_controllers/JointPositionController + joint: HeadPitch + + LShoulderPitch_position_controller: + type: position_controllers/JointPositionController + joint: LShoulderPitch + + LElbowYaw_position_controller: + type: position_controllers/JointPositionController + joint: LElbowYaw + + LWristYaw_position_controller: + type: position_controllers/JointPositionController + joint: LWristYaw + + LHand_position_controller: + type: position_controllers/JointPositionController + joint: LHand + + LShoulderRoll_position_controller: + type: position_controllers/JointPositionController + joint: LShoulderRoll + + LElbowRoll_position_controller: + type: position_controllers/JointPositionController + joint: LElbowRoll + + RShoulderPitch_position_controller: + type: position_controllers/JointPositionController + joint: RShoulderPitch + + RElbowYaw_position_controller: + type: position_controllers/JointPositionController + joint: RElbowYaw + + RWristYaw_position_controller: + type: position_controllers/JointPositionController + joint: RWristYaw + + RHand_position_controller: + type: position_controllers/JointPositionController + joint: RHand + + RShoulderRoll_position_controller: + type: position_controllers/JointPositionController + joint: RShoulderRoll + + RElbowRoll_position_controller: + type: position_controllers/JointPositionController + joint: RElbowRoll + + LHipYawPitch_position_controller: + type: position_controllers/JointPositionController + joint: LHipYawPitch + + LHipRoll_position_controller: + type: position_controllers/JointPositionController + joint: LHipRoll + + LHipPitch_position_controller: + type: position_controllers/JointPositionController + joint: LHipPitch + + LKneePitch_position_controller: + type: position_controllers/JointPositionController + joint: LKneePitch + + LAnklePitch_position_controller: + type: position_controllers/JointPositionController + joint: LAnklePitch + + LAnkleRoll_position_controller: + type: position_controllers/JointPositionController + joint: LAnkleRoll + + RHipYawPitch_position_controller: + type: position_controllers/JointPositionController + joint: RHipYawPitch + + RHipRoll_position_controller: + type: position_controllers/JointPositionController + joint: RHipRoll + + RHipPitch_position_controller: + type: position_controllers/JointPositionController + joint: RHipPitch + + RKneePitch_position_controller: + type: position_controllers/JointPositionController + joint: RKneePitch + + RAnklePitch_position_controller: + type: position_controllers/JointPositionController + joint: RAnklePitch + + RAnkleRoll_position_controller: + type: position_controllers/JointPositionController + joint: RAnkleRoll + + diff --git a/nao_dcm_common/nao_dcm_control/config/nao_trajectory_control.yaml b/nao_dcm_common/nao_dcm_control/config/nao_trajectory_control.yaml new file mode 100644 index 0000000..746987e --- /dev/null +++ b/nao_dcm_common/nao_dcm_control/config/nao_trajectory_control.yaml @@ -0,0 +1,61 @@ +nao_dcm: + # Publish all joint states ----------------------------------- + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + + # Controllers ------------------------------------------------ + Head_controller: + type: position_controllers/JointTrajectoryController + joints: + - HeadYaw + - HeadPitch + LeftArm_controller: + type: position_controllers/JointTrajectoryController + joints: + - LShoulderPitch + - LShoulderRoll + - LElbowYaw + - LElbowRoll + RightArm_controller: + type: position_controllers/JointTrajectoryController + joints: + - RShoulderPitch + - RShoulderRoll + - RElbowYaw + - RElbowRoll + LeftHand_controller: + type: position_controllers/JointTrajectoryController + joints: + - LWristYaw + - LHand + RightHand_controller: + type: position_controllers/JointTrajectoryController + joints: + - RWristYaw + - RHand + Pelvis_controller: + type: position_controllers/JointTrajectoryController + joints: + - LHipYawPitch + LeftLeg_controller: + type: position_controllers/JointTrajectoryController + joints: + - LHipRoll + - LHipPitch + - LKneePitch + - LAnklePitch + - LAnkleRoll + RightLeg_controller: + type: position_controllers/JointTrajectoryController + joints: + - RHipRoll + - RHipPitch + - RKneePitch + - RAnklePitch + - RAnkleRoll + Head_controller: + type: position_controllers/JointTrajectoryController + joints: + - HeadYaw + - HeadPitch diff --git a/nao_dcm_common/nao_dcm_control/config/nao_trajectory_control_H21.yaml b/nao_dcm_common/nao_dcm_control/config/nao_trajectory_control_H21.yaml new file mode 100644 index 0000000..e50c6a8 --- /dev/null +++ b/nao_dcm_common/nao_dcm_control/config/nao_trajectory_control_H21.yaml @@ -0,0 +1,51 @@ +nao_dcm: + # Publish all joint states ----------------------------------- + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + + # Controllers ------------------------------------------------ + Head_controller: + type: position_controllers/JointTrajectoryController + joints: + - HeadYaw + - HeadPitch + LeftArm_controller: + type: position_controllers/JointTrajectoryController + joints: + - LShoulderPitch + - LShoulderRoll + - LElbowYaw + - LElbowRoll + RightArm_controller: + type: position_controllers/JointTrajectoryController + joints: + - RShoulderPitch + - RShoulderRoll + - RElbowYaw + - RElbowRoll + Pelvis_controller: + type: position_controllers/JointTrajectoryController + joints: + - LHipYawPitch + LeftLeg_controller: + type: position_controllers/JointTrajectoryController + joints: + - LHipRoll + - LHipPitch + - LKneePitch + - LAnklePitch + - LAnkleRoll + RightLeg_controller: + type: position_controllers/JointTrajectoryController + joints: + - RHipRoll + - RHipPitch + - RKneePitch + - RAnklePitch + - RAnkleRoll + Head_controller: + type: position_controllers/JointTrajectoryController + joints: + - HeadYaw + - HeadPitch diff --git a/nao_dcm_common/nao_dcm_control/launch/nao_dcm_position_control.launch b/nao_dcm_common/nao_dcm_control/launch/nao_dcm_position_control.launch new file mode 100644 index 0000000..bafe2d5 --- /dev/null +++ b/nao_dcm_common/nao_dcm_control/launch/nao_dcm_position_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_control/launch/nao_dcm_trajectory_control_H21.launch b/nao_dcm_common/nao_dcm_control/launch/nao_dcm_trajectory_control_H21.launch new file mode 100644 index 0000000..cc55426 --- /dev/null +++ b/nao_dcm_common/nao_dcm_control/launch/nao_dcm_trajectory_control_H21.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_control/launch/nao_dcm_trajectory_control_H25.launch b/nao_dcm_common/nao_dcm_control/launch/nao_dcm_trajectory_control_H25.launch new file mode 100644 index 0000000..0f1f8de --- /dev/null +++ b/nao_dcm_common/nao_dcm_control/launch/nao_dcm_trajectory_control_H25.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_control/package.xml b/nao_dcm_common/nao_dcm_control/package.xml new file mode 100644 index 0000000..2680eea --- /dev/null +++ b/nao_dcm_common/nao_dcm_control/package.xml @@ -0,0 +1,37 @@ + + + + nao_dcm_control + 0.0.1 + Control for Aldebaran's NAO robot (v4) + + Konstantinos Chatzilygeroudis + + BSD + + https://github.com/costashatz/nao_dcm + https://github.com/costashatz/nao_dcm/issues + + Konstantinos Chatzilygeroudis + + catkin + + robot_state_publisher + ros_control + ros_controllers + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/CMakeLists.txt b/nao_dcm_common/nao_dcm_description/CMakeLists.txt new file mode 100644 index 0000000..21cc0a0 --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(nao_dcm_description) + +## Find catkin +find_package(catkin REQUIRED) + +##Needed for ros packages +catkin_package(CATKIN_DEPENDS xacro) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY urdf + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/config/nao_dcm.rviz b/nao_dcm_common/nao_dcm_description/config/nao_dcm.rviz new file mode 100644 index 0000000..31bf105 --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/config/nao_dcm.rviz @@ -0,0 +1,262 @@ +Panels: + - Class: rviz/Displays + Help Height: 81 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 720 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + CameraBottom: + Alpha: 1 + Show Axes: false + Show Trail: false + CameraTop: + Alpha: 1 + Show Axes: false + Show Trail: false + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + LElbow: + Alpha: 1 + Show Axes: false + Show Trail: false + LElbowDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LHip: + Alpha: 1 + Show Axes: false + Show Trail: false + LHipDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LHipYaw: + Alpha: 1 + Show Axes: false + Show Trail: false + LKnee: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LShoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + LShoulderDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RElbow: + Alpha: 1 + Show Axes: false + Show Trail: false + RElbowDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RHip: + Alpha: 1 + Show Axes: false + Show Trail: false + RHipDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RHipYaw: + Alpha: 1 + Show Axes: false + Show Trail: false + RKnee: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RShoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + RShoulderDummy: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + SonarLeft: + Alpha: 1 + Show Axes: false + Show Trail: false + SonarRight: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + gaze: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_ankle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_sole: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + neck: + Alpha: 1 + Show Axes: false + Show Trail: false + r_ankle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_sole: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.44466 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.355398 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.755394 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1031 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c00000362fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000362000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000362fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004300000362000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000023a00fffffffb0000000800540069006d00650100000000000004500000000000000000000005290000036200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: -2 + Y: 25 diff --git a/nao_dcm_common/nao_dcm_description/launch/display_nao_dcm_H21.launch b/nao_dcm_common/nao_dcm_description/launch/display_nao_dcm_H21.launch new file mode 100644 index 0000000..cd6bc55 --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/launch/display_nao_dcm_H21.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/launch/display_nao_dcm_H25.launch b/nao_dcm_common/nao_dcm_description/launch/display_nao_dcm_H25.launch new file mode 100644 index 0000000..ac26555 --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/launch/display_nao_dcm_H25.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/launch/nao_dcm_H21_urdf.launch b/nao_dcm_common/nao_dcm_description/launch/nao_dcm_H21_urdf.launch new file mode 100644 index 0000000..6a00158 --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/launch/nao_dcm_H21_urdf.launch @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/launch/nao_dcm_H25_urdf.launch b/nao_dcm_common/nao_dcm_description/launch/nao_dcm_H25_urdf.launch new file mode 100644 index 0000000..49e83bf --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/launch/nao_dcm_H25_urdf.launch @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/package.xml b/nao_dcm_common/nao_dcm_description/package.xml new file mode 100644 index 0000000..3e108bb --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/package.xml @@ -0,0 +1,39 @@ + + + + nao_dcm_description + 0.0.1 + URDF for Aldebaran's NAO robot (v4) + + Konstantinos Chatzilygeroudis + + BSD + + https://github.com/costashatz/nao_dcm + https://github.com/costashatz/nao_dcm/issues + + Konstantinos Chatzilygeroudis + + catkin + + robot_state_publisher + joint_state_publisher + rviz + xacro + urdf + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21.xacro new file mode 100644 index 0000000..48c5827 --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21.xacro @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_control.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_control.xacro new file mode 100644 index 0000000..c447add --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_control.xacro @@ -0,0 +1,124 @@ + + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 150.27 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 173.22 + + + + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 150.27 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 150.27 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 173.22 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 173.22 + + + + + + + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 201.3 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 201.3 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 130.85 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 130.85 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 130.85 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 201.3 + + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_structure.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_structure.xacro new file mode 100644 index 0000000..202225a --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_structure.xacro @@ -0,0 +1,412 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25.xacro new file mode 100644 index 0000000..ed6352b --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25.xacro @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_control.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_control.xacro new file mode 100644 index 0000000..35ed5e1 --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_control.xacro @@ -0,0 +1,142 @@ + + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 150.27 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 173.22 + + + + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 150.27 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 150.27 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 50.61 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 36.24 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 173.22 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 173.22 + + + + + + + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 201.3 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 201.3 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 130.85 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 130.85 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 130.85 + + + + transmission_interface/SimpleTransmission + + + + PositionJointInterface + 201.3 + + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_structure.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_structure.xacro new file mode 100644 index 0000000..ad66c85 --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_structure.xacro @@ -0,0 +1,430 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro new file mode 100644 index 0000000..39f6d2e --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro @@ -0,0 +1,280 @@ + + + + + + /nao_dcm + gazebo_ros_control/DefaultRobotHWSim + + + + + + Gazebo/Blue + 0.5 + 0.5 + false + false + + + Gazebo/Red + 0.5 + 0.5 + false + false + + + Gazebo/Green + 0.5 + 0.5 + false + false + + + Gazebo/Green + 0.5 + 0.5 + false + false + + + Gazebo/Green + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + Gazebo/Red + 0.5 + 0.5 + false + false + + + Gazebo/Yellow + 0.5 + 0.5 + false + false + + + Gazebo/Yellow + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + 0.5 + 0.5 + false + false + + + Gazebo/Red + 0.5 + 0.5 + false + false + + + Gazebo/Yellow + 0.5 + 0.5 + false + false + + + Gazebo/Yellow + 0.5 + 0.5 + false + false + + + Gazebo/Green + 0.5 + 0.5 + false + false + + + Gazebo/Green + 0.5 + 0.5 + false + false + + + Gazebo/Blue + 0.5 + 0.5 + false + false + + + Gazebo/Green + 0.5 + 0.5 + false + false + + + Gazebo/Green + 0.5 + 0.5 + false + false + + + Gazebo/Blue + 0.5 + 0.5 + false + false + 1 0 0 + 1.0 + 0.003 + + + Gazebo/Green + 0.5 + 0.5 + false + false + + + Gazebo/Green + 0.5 + 0.5 + false + false + + + Gazebo/Blue + 0.5 + 0.5 + false + false + + + Gazebo/Green + 0.5 + 0.5 + false + false + + + Gazebo/Green + 0.5 + 0.5 + false + false + + + Gazebo/Blue + 0.5 + 0.5 + false + false + 1 0 0 + 1.0 + 0.003 + + + + + + + + 30.0 + + 1.06290551 + + 640 + 480 + R8G8B8 + + + 0.3 + 500 + + + gaussian + 0.0 + 0.007 + + + + true + 0.0 + nao_robot/CameraTop + image_raw + camera_info + CameraTop + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + 30.0 + + 0.834267382 + + 640 + 480 + R8G8B8 + + + 0.3 + 500 + + + gaussian + 0.0 + 0.007 + + + + true + 0.0 + nao_robot/CameraBottom + image_raw + camera_info + CameraBottom + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + \ No newline at end of file diff --git a/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_visuals_collisions.xacro b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_visuals_collisions.xacro new file mode 100644 index 0000000..7ef647e --- /dev/null +++ b/nao_dcm_common/nao_dcm_description/urdf/nao_dcm_visuals_collisions.xacro @@ -0,0 +1,181 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_bringup/CMakeLists.txt b/nao_dcm_robot/nao_dcm_bringup/CMakeLists.txt new file mode 100644 index 0000000..14fa42f --- /dev/null +++ b/nao_dcm_robot/nao_dcm_bringup/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 2.8.3) +project(nao_dcm_bringup) + +## Find catkin macros and libraries +find_package(catkin REQUIRED) +find_package(Boost) + +##Needed for ros packages +catkin_package(CATKIN_DEPENDS rospy nao_dcm_driver) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/nao_dcm_robot/nao_dcm_bringup/config/nao_dcm.yaml b/nao_dcm_robot/nao_dcm_bringup/config/nao_dcm.yaml new file mode 100644 index 0000000..fde1071 --- /dev/null +++ b/nao_dcm_robot/nao_dcm_bringup/config/nao_dcm.yaml @@ -0,0 +1,36 @@ +# DO NOT CHANGE IT +Nao_Version: V4 + +# Choose what components are enabled on startup +Nao_SonarEnabled: true +Nao_TactilesEnabled: true +Nao_BumpersEnabled: true +Nao_FootContactsEnabled: true + +# Camera related parameters +Nao_UseCamera: true +Nao_TopCameraEnabled: true +Nao_BottomCameraEnabled: true + +# Application related parameters +Nao_PublishIMU: true + +# ROS related paramaters +Nao_TopicQueue: 10 + +# Robot-Connection related parameters +Nao_RobotPort: 9559 +Nao_RobotIP: nao.local +Nao_DriverBrokerPort: 54000 +Nao_DriverBrokerIP: 0.0.0.0 +Nao_CameraBrokerPort: 54001 +Nao_CameraBrokerIP: 0.0.0.0 + +# Communication related parameters +Nao_HighCommunicationFrequency: 50 +Nao_LowCommunicationFrequency: 10 +Nao_ControllerFrequency: 10 +Nao_CameraFrequency: 15 + +# DO NOT CHANGE IT +Nao_Prefix: nao_dcm \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup.launch b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup.launch new file mode 100644 index 0000000..56df3e1 --- /dev/null +++ b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup_remote.launch b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup_remote.launch new file mode 100644 index 0000000..c94a997 --- /dev/null +++ b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H21_bringup_remote.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup.launch b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup.launch new file mode 100644 index 0000000..602fe7e --- /dev/null +++ b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup_remote.launch b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup_remote.launch new file mode 100644 index 0000000..d49b861 --- /dev/null +++ b/nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_H25_bringup_remote.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_bringup/package.xml b/nao_dcm_robot/nao_dcm_bringup/package.xml new file mode 100644 index 0000000..3e7a846 --- /dev/null +++ b/nao_dcm_robot/nao_dcm_bringup/package.xml @@ -0,0 +1,38 @@ + + + + nao_dcm_bringup + 0.0.1 + Bring-up the nao_dcm driver to connect to Aldebaran's Nao robot (v4). + + Konstantinos Chatzilygeroudis + + + BSD + + https://github.com/costashatz/nao_dcm + https://github.com/costashatz/nao_dcm/issues + + Konstantinos Chatzilygeroudis + + + catkin + + rospy + nao_dcm_driver + + + + + \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_driver/CMakeLists.txt b/nao_dcm_robot/nao_dcm_driver/CMakeLists.txt new file mode 100644 index 0000000..4d84abd --- /dev/null +++ b/nao_dcm_robot/nao_dcm_driver/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 2.8.3) +project(nao_dcm_driver) + +# Tell CMake where to find "FindNAOqi.cmake" +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + geometry_msgs + tf + hardware_interface + controller_manager + nao_dcm_msgs + sensor_msgs +) + +find_package(NAOqi QUIET COMPONENTS + alcommon + alvision + alproxies + alerror + alvalue +) + +find_package(Boost) + +##Needed for ros packages +catkin_package(CATKIN_DEPENDS roscpp geometry_msgs tf std_msgs nao_dcm_msgs sensor_msgs hardware_interface controller_manager) + +if( NAOqi_FOUND AND Boost_FOUND) + message(STATUS "NAOqi C++ SDK found; C++ nodes will be built") + include_directories( include + ${catkin_INCLUDE_DIRS} + ${NAOqi_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS}) + add_executable(nao_dcm_driver src/nao_driver.cpp src/nao.cpp) + target_link_libraries(nao_dcm_driver + ${catkin_LIBRARIES} + ${NAOqi_LIBRARIES} + ${Boost_LIBRARIES}) + add_dependencies(nao_dcm_driver ${catkin_EXPORTED_TARGETS}) +else() + message(STATUS "Cannot find NAOqi C++ sdk; C++ nodes will NOT be built") +endif() + + +install(TARGETS nao_dcm_driver + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/nao_dcm_robot/nao_dcm_driver/cmake/FindNAOqi.cmake b/nao_dcm_robot/nao_dcm_driver/cmake/FindNAOqi.cmake new file mode 100644 index 0000000..5d03579 --- /dev/null +++ b/nao_dcm_robot/nao_dcm_driver/cmake/FindNAOqi.cmake @@ -0,0 +1,149 @@ +# Copyright (c) 2013, Miguel Sarabia +# Imperial College London +# +# 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 the Imperial College London 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. +# + + +# - Try to find NAOqi +# Once executed this script will define the following: +# NAOqi_FOUND - NAOqi was succesfully found +# NAOqi_INCLUDE_DIRS - NAOqi's include directories +# NAOqi_LIBRARIES - NAOqi's libraries +# NAOqi_DIR - Directory where NAOqi was found +#------------------------------------------------------------------------------ +# Users can set NAOqi_DIR to force CMake to look in a particular location, +# setting the AL_DIR environment variable will have a similar effect. + +cmake_minimum_required(VERSION 2.8.3) + +#These are NAOqi's known components (ie. libraries) +set(NAOqi_COMPONENTS + alaudio + albonjourdiscovery + alextractor + allog + almodelutils + alproject + alresource + altools + alautomatictest + alboxrary + alfile + almathinternal + almotion + alpythonbridge + alserial + altts + albehaviorinfo + alcommon + allauncher + almath + almotionrecorder + alpythontools + alsoap + alvalue + albehavior + alerror + allogremote + almemoryfastaccess + alparammanager + alremotecall + althread + alvision + alproxies) + + +#Set INCLUDE hints +set(NAOqi_INCLUDE_HINTS + "${NAOqi_DIR}/include" + "$ENV{AL_DIR}/include" ) + +# Set LIBRARY hints +set(NAOqi_LIBRARY_HINTS + "${NAOqi_DIR}/lib" + "$ENV{AL_DIR}/lib" ) + +# Find include directories +find_path(NAOqi_INCLUDE_DIR alcommon/alproxy.h HINTS ${NAOqi_INCLUDE_HINTS} ) + +# Verify we know about all the components requested +# and remove those we don't know about +set(NAOqi_FILTERED_COMPONENTS ${NAOqi_FIND_COMPONENTS}) + +if ( NAOqi_FIND_COMPONENTS ) + foreach(comp ${NAOqi_FIND_COMPONENTS}) + list(FIND NAOqi_COMPONENTS ${comp} ${comp}_KNOWN) + if (${comp}_KNOWN EQUAL -1) + list(REMOVE_ITEM NAOqi_FILTERED_COMPONENTS ${comp}) + message(STATUS "Unknown NAOqi component ${comp}") + endif() + endforeach() +endif() + +list(LENGTH NAOqi_FILTERED_COMPONENTS NAOqi_NUMBER_OF_COMPONENTS) +set(NAOqi_FOUND_COMPONENTS TRUE) + +# Look for components (ie. libraries) +if( ${NAOqi_NUMBER_OF_COMPONENTS} ) + foreach(comp ${NAOqi_FILTERED_COMPONENTS}) + #Look for the actual library here + find_library(${comp}_LIBRARY NAMES ${comp} HINTS ${NAOqi_LIBRARY_HINTS}) + if ( ${${comp}_LIBRARY} STREQUAL ${comp}_LIBRARY-NOTFOUND) + message(STATUS "Could not find NAOqi's ${comp}") + set(NAOqi_FOUND_COMPONENTS FALSE) + else() + #If everything went well append this component to list of libraries + list(APPEND NAOqi_LIBRARY ${${comp}_LIBRARY}) + endif() + endforeach() +else() + message(STATUS "No NAOqi components specified") +endif() + + +# Handle the QUIET and REQUIRED arguments +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args( + NAOqi #Package name + DEFAULT_MSG + # Variables required to evaluate as TRUE + NAOqi_LIBRARY + NAOqi_INCLUDE_DIR + NAOqi_FOUND_COMPONENTS) + +# Copy the values of the advanced variables to the user-facing ones +set(NAOqi_LIBRARIES ${NAOqi_LIBRARY} ) +set(NAOqi_INCLUDE_DIRS ${NAOqi_INCLUDE_DIR} ) +set(NAOqi_FOUND ${NAOQI_FOUND}) + +# If NAOqi was found, update NAOqi_DIR to show where it was found +if ( NAOqi_FOUND ) + get_filename_component(NAOqi_NEW_DIR "${NAOqi_INCLUDE_DIRS}/../" ABSOLUTE) +endif() +set(NAOqi_DIR ${NAOqi_NEW_DIR} CACHE FILEPATH "NAOqi root directory" FORCE) + +#Hide these variables +mark_as_advanced(NAOqi_INCLUDE_DIR NAOqi_LIBRARY NAOQI_FOUND) diff --git a/nao_dcm_robot/nao_dcm_driver/config/nao_dcm_analyzers.yaml b/nao_dcm_robot/nao_dcm_driver/config/nao_dcm_analyzers.yaml new file mode 100644 index 0000000..579ab1e --- /dev/null +++ b/nao_dcm_robot/nao_dcm_driver/config/nao_dcm_analyzers.yaml @@ -0,0 +1,17 @@ +pub_rate: 1.0 # Optional +base_path: "" # Optional +analyzers: + nao_dcm_driver: + type: AnalyzerGroup + path: Nao + analyzers: + battery: + type: GenericAnalyzer + path: Battery + remove_prefix: nao_dcm_driver + contains: Battery + joints: + type: GenericAnalyzer + path: Joints + remove_prefix: nao_dcm_driver + contains: Joints diff --git a/nao_dcm_robot/nao_dcm_driver/include/nao_dcm_driver/nao.h b/nao_dcm_robot/nao_dcm_driver/include/nao_dcm_driver/nao.h new file mode 100644 index 0000000..8f1c8d9 --- /dev/null +++ b/nao_dcm_robot/nao_dcm_driver/include/nao_dcm_driver/nao.h @@ -0,0 +1,231 @@ +/**Copyright (c) 2014, Konstantinos Chatzilygeroudis +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. 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. + +3. Neither the name of the copyright holder 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 HOLDER 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. +**/ + +#ifndef NAO_DCM_DRIVER_NAO_H +#define NAO_DCM_DRIVER_NAO_H + +// Boost Headers +#include +#include +#include + +// NAOqi Headers +#include +#include +#include +#include +#include +#include + +// ROS Headers +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#include + +using std::string; +using std::vector; + +namespace AL +{ +class ALBroker; +} + +class Nao : public AL::ALModule, public hardware_interface::RobotHW +{ +private: + // ROS Standard Variables + ros::NodeHandle node_handle_; + + // ROS Topics/Messages + ros::Subscriber cmd_vel_sub_; + + ros::Publisher imu_pub_; + sensor_msgs::Imu imu_; + + tf::TransformBroadcaster base_footprint_broadcaster_; + tf::TransformListener base_footprint_listener_; + + ros::Publisher sonar_left_pub_, sonar_right_pub_; + sensor_msgs::Range sonar_left_, sonar_right_; + ros::ServiceServer sonar_switch_; + + ros::Publisher fsrs_pub_; + nao_dcm_msgs::FSRs fsrs_; + ros::ServiceServer fsrs_switch_; + + ros::Publisher bumpers_pub_, tactiles_pub_; + nao_dcm_msgs::Bumper bumpers_; + nao_dcm_msgs::Tactile tactiles_; + ros::ServiceServer bumpers_switch_, tactiles_switch_; + + ros::Publisher stiffness_pub_; + std_msgs::Float32 stiffness_; + ros::ServiceServer stiffness_switch_; + + controller_manager::ControllerManager* manager_; + + // ROS Diagnostics + diagnostic_updater::Updater diagnostic_; + + // Member Variables + AL::ALValue commands_; + + // Helper + bool is_connected_; + + // Robot Parameters + string version_, body_type_; + bool sonar_enabled_, tactiles_enabled_, bumpers_enabled_, foot_contacts_enabled_; + bool imu_published_, stiffnesses_enabled_; + int topic_queue_; + string prefix_; + double low_freq_, high_freq_, controller_freq_; + + // AL Proxies + AL::ALMemoryProxy memory_proxy_; + AL::DCMProxy dcm_proxy_; + + // IMU + AL::ALValue imu_names_; + // Sonars + AL::ALValue sonar_names_; + // FSRs + AL::ALValue fsr_names_; + // Tactile + AL::ALValue tactile_names_; + // Bumper + AL::ALValue bumper_names_; + // Joints + AL::ALValue joints_names_; + AL::ALValue joint_temperature_names_; + // Battery + AL::ALValue battery_names_; + // LEDs + AL::ALValue led_names_; + + // Joint States + hardware_interface::JointStateInterface jnt_state_interface_; + hardware_interface::PositionJointInterface jnt_pos_interface_; + + int number_of_joints_; + vector joint_names_; + vector joint_commands_; + vector joint_angles_; + vector joint_velocities_; + vector joint_efforts_; +public: + // Constructor/Destructor + Nao(boost::shared_ptr broker, const std::string& name); + ~Nao(); + + bool initialize(); + bool initializeControllers(controller_manager::ControllerManager& cm); + + // Connect/Disconnet to ALProxies + bool connect(const ros::NodeHandle nh); + void disconnect(); + + // Subscribe/Advertise to ROS Topics/Services + void subscribe(); + + // Parameter Server + void loadParams(); + + // Helper + void brokerDisconnected(const string& event_name, const string &broker_name, const string& subscriber_identifier); + + // DCMProxy Wrapper Methods + void DCMTimedCommand(const string& key, const AL::ALValue& value, const int& timeOffset, + const string& type="Merge"); + void DCMAliasTimedCommand(const string& alias, const vector& values, const vector& timeOffsets, + const string& type="Merge", const string& type2="time-mixed"); + + // ALMemoryProxy Wrapper Methods + void insertDataToMemory(const string& key, const AL::ALValue& value); + AL::ALValue getDataFromMemory(const string& key); + void subscribeToEvent(const std::string& name, const std::string& callback_module, + const std::string& callback_method); + void subscribeToMicroEvent(const std::string& name, const std::string& callback_module, + const std::string& callback_method, const string& callback_message=""); + void unsubscribeFromEvent(const string& name, const string& callback_module); + void unsubscribeFromMicroEvent(const string& name, const string& callback_module); + void raiseEvent(const string& name, const AL::ALValue& value); + void raiseMicroEvent(const string& name, const AL::ALValue& value); + void declareEvent(const string& name); + + // General Methods + void controllerLoop(); + void lowCommunicationLoop(); + void highCommunicationLoop(); + + bool connected(); + + // ROS Callbacks/Related Methods + void commandVelocity(const geometry_msgs::TwistConstPtr &msg); + + void publishIMU(const ros::Time &ts); + + void publishBaseFootprint(const ros::Time &ts); + + void readJoints(); + + void writeJoints(); + + bool switchSonar(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res); + + bool switchFSR(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res); + + bool switchBumper(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res); + + bool switchTactile(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res); + + bool switchStiffnesses(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res); + + void checkSonar(); + + void checkFSR(); + + void checkTactile(); + + void checkBumper(); + + void checkTemperature(diagnostic_updater::DiagnosticStatusWrapper &stat); + + void checkBattery(diagnostic_updater::DiagnosticStatusWrapper &stat); + + void run(); + +}; + +#endif // NAO_H diff --git a/nao_dcm_robot/nao_dcm_driver/package.xml b/nao_dcm_robot/nao_dcm_driver/package.xml new file mode 100644 index 0000000..92c7abf --- /dev/null +++ b/nao_dcm_robot/nao_dcm_driver/package.xml @@ -0,0 +1,56 @@ + + + + nao_dcm_driver + 0.0.1 + Package containing the hardware interface to connect to Aldebaran's Nao robot (v4). + + Konstantinos Chatzilygeroudis + + + BSD + + https://github.com/costashatz/nao_dcm + https://github.com/costashatz/nao_dcm/issues + + Konstantinos Chatzilygeroudis + + + catkin + + roscpp + rospy + std_msgs + controller_manager + hardware_interface + geometry_msgs + tf + nao_dcm_msgs + sensor_msgs + + + roscpp + rospy + std_msgs + geometry_msgs + controller_manager + hardware_interface + tf + nao_dcm_msgs + sensor_msgs + + + + + \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_driver/src/nao.cpp b/nao_dcm_robot/nao_dcm_driver/src/nao.cpp new file mode 100644 index 0000000..740b35d --- /dev/null +++ b/nao_dcm_robot/nao_dcm_driver/src/nao.cpp @@ -0,0 +1,1074 @@ +/**Copyright (c) 2014, Konstantinos Chatzilygeroudis +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. 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. + +3. Neither the name of the copyright holder 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 HOLDER 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. +**/ + +#include +#include "nao_dcm_driver/nao.h" +#include +#include +#include + +Nao::Nao(boost::shared_ptr broker, const string &name) + : AL::ALModule(broker,name),is_connected_(false) +{ + setModuleDescription("Nao Robot Module"); + + functionName("brokerDisconnected", getName(), "Callback when broker disconnects!"); + BIND_METHOD(Nao::brokerDisconnected); +} + +Nao::~Nao() +{ + if(is_connected_) + disconnect(); +} + +bool Nao::initialize() +{ + // IMU Memory Keys + imu_names_ = AL::ALValue::array("Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value", + "Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value", + "Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value", + "Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value", + "Device/SubDeviceList/InertialSensor/GyroscopeY/Sensor/Value", + "Device/SubDeviceList/InertialSensor/GyroscopeZ/Sensor/Value", + "Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value", + "Device/SubDeviceList/InertialSensor/AccelerometerY/Sensor/Value", + "Device/SubDeviceList/InertialSensor/AccelerometerZ/Sensor/Value"); + + // Sonar Memory Keys + sonar_names_ = AL::ALValue::array("Device/SubDeviceList/US/Left/Sensor/Value", + "Device/SubDeviceList/US/Left/Sensor/Value1", + "Device/SubDeviceList/US/Left/Sensor/Value2", + "Device/SubDeviceList/US/Left/Sensor/Value3", + "Device/SubDeviceList/US/Left/Sensor/Value4", + "Device/SubDeviceList/US/Left/Sensor/Value5", + "Device/SubDeviceList/US/Left/Sensor/Value6", + "Device/SubDeviceList/US/Left/Sensor/Value7", + "Device/SubDeviceList/US/Left/Sensor/Value8", + "Device/SubDeviceList/US/Left/Sensor/Value9"); + sonar_names_.arrayPush("Device/SubDeviceList/US/Right/Sensor/Value"); + sonar_names_.arrayPush("Device/SubDeviceList/US/Right/Sensor/Value1"); + sonar_names_.arrayPush("Device/SubDeviceList/US/Right/Sensor/Value2"); + sonar_names_.arrayPush("Device/SubDeviceList/US/Right/Sensor/Value3"); + sonar_names_.arrayPush("Device/SubDeviceList/US/Right/Sensor/Value4"); + sonar_names_.arrayPush("Device/SubDeviceList/US/Right/Sensor/Value5"); + sonar_names_.arrayPush("Device/SubDeviceList/US/Right/Sensor/Value6"); + sonar_names_.arrayPush("Device/SubDeviceList/US/Right/Sensor/Value7"); + sonar_names_.arrayPush("Device/SubDeviceList/US/Right/Sensor/Value8"); + sonar_names_.arrayPush("Device/SubDeviceList/US/Right/Sensor/Value9"); + + // Foot Contact Memory Keys + fsr_names_ = AL::ALValue::array("Device/SubDeviceList/LFoot/FSR/FrontLeft/Sensor/Value", + "Device/SubDeviceList/LFoot/FSR/FrontRight/Sensor/Value", + "Device/SubDeviceList/LFoot/FSR/RearLeft/Sensor/Value", + "Device/SubDeviceList/LFoot/FSR/RearRight/Sensor/Value", + "Device/SubDeviceList/LFoot/FSR/TotalWeight/Sensor/Value", + "Device/SubDeviceList/RFoot/FSR/FrontLeft/Sensor/Value", + "Device/SubDeviceList/RFoot/FSR/FrontRight/Sensor/Value", + "Device/SubDeviceList/RFoot/FSR/RearLeft/Sensor/Value", + "Device/SubDeviceList/RFoot/FSR/RearRight/Sensor/Value", + "Device/SubDeviceList/RFoot/FSR/TotalWeight/Sensor/Value"); + fsr_names_.arrayPush("Device/SubDeviceList/LFoot/FSR/CenterOfPressure/X/Sensor/Value"); + fsr_names_.arrayPush("Device/SubDeviceList/LFoot/FSR/CenterOfPressure/Y/Sensor/Value"); + fsr_names_.arrayPush("Device/SubDeviceList/RFoot/FSR/CenterOfPressure/X/Sensor/Value"); + fsr_names_.arrayPush("Device/SubDeviceList/RFoot/FSR/CenterOfPressure/Y/Sensor/Value"); + + // Tactile Memory Keys + tactile_names_ = AL::ALValue::array("Device/SubDeviceList/Head/Touch/Front/Sensor/Value", + "Device/SubDeviceList/Head/Touch/Middle/Sensor/Value", + "Device/SubDeviceList/Head/Touch/Rear/Sensor/Value", + "Device/SubDeviceList/LHand/Touch/Back/Sensor/Value", + "Device/SubDeviceList/LHand/Touch/Left/Sensor/Value", + "Device/SubDeviceList/LHand/Touch/Right/Sensor/Value", + "Device/SubDeviceList/RHand/Touch/Back/Sensor/Value", + "Device/SubDeviceList/RHand/Touch/Left/Sensor/Value", + "Device/SubDeviceList/RHand/Touch/Right/Sensor/Value"); + + // Bumper Memory Keys + bumper_names_ = AL::ALValue::array("Device/SubDeviceList/LFoot/Bumper/Left/Sensor/Value", + "Device/SubDeviceList/LFoot/Bumper/Right/Sensor/Value", + "Device/SubDeviceList/RFoot/Bumper/Left/Sensor/Value", + "Device/SubDeviceList/RFoot/Bumper/Right/Sensor/Value"); + + // Battery Memory Keys + battery_names_ = AL::ALValue::array("Device/SubDeviceList/Battery/Charge/Sensor/Value", + "Device/SubDeviceList/Battery/Temperature/Sensor/Value"); + + // LED Memory Keys + led_names_.arrayPush("Device/SubDeviceList/ChestBoard/Led/Blue/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/ChestBoard/Led/Green/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/ChestBoard/Led/Red/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Left/0Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Left/108Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Left/144Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Left/180Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Left/216Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Left/252Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Left/288Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Left/324Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Left/36Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Left/72Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Right/0Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Right/108Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Right/144Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Right/180Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Right/216Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Right/252Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Right/288Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Right/324Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Right/36Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Ears/Led/Right/72Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Left/0Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Left/135Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Left/180Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Left/225Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Left/270Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Left/315Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Left/45Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Left/90Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Right/0Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Right/135Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Right/180Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Right/225Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Right/270Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Right/315Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Right/45Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Blue/Right/90Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Left/0Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Left/135Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Left/180Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Left/225Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Left/270Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Left/315Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Left/45Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Left/90Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Right/0Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Right/135Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Right/180Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Right/225Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Right/270Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Right/315Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Right/45Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Green/Right/90Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Left/0Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Left/135Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Left/180Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Left/225Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Left/270Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Left/315Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Left/45Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Left/90Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Right/0Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Right/135Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Right/180Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Right/225Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Right/270Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Right/315Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Right/45Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Face/Led/Red/Right/90Deg/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Head/Led/Front/Left/0/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Head/Led/Front/Left/1/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Head/Led/Front/Right/0/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Head/Led/Front/Right/1/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Head/Led/Middle/Left/0/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Head/Led/Middle/Right/0/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Head/Led/Rear/Left/0/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Head/Led/Rear/Left/1/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Head/Led/Rear/Left/2/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Head/Led/Rear/Right/0/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Head/Led/Rear/Right/1/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/Head/Led/Rear/Right/2/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/LFoot/Led/Blue/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/LFoot/Led/Green/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/LFoot/Led/Red/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/RFoot/Led/Blue/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/RFoot/Led/Green/Actuator/Value"); + led_names_.arrayPush("Device/SubDeviceList/RFoot/Led/Red/Actuator/Value"); + + // Joints Initialization + joint_names_ = vector(); + joint_names_.push_back("HeadYaw"); + joint_names_.push_back("HeadPitch"); + joint_names_.push_back("LShoulderPitch"); + joint_names_.push_back("LShoulderRoll"); + joint_names_.push_back("LElbowYaw"); + joint_names_.push_back("LElbowRoll"); + joint_names_.push_back("LWristYaw"); + joint_names_.push_back("LHand"); + joint_names_.push_back("RShoulderPitch"); + joint_names_.push_back("RShoulderRoll"); + joint_names_.push_back("RElbowYaw"); + joint_names_.push_back("RElbowRoll"); + joint_names_.push_back("RWristYaw"); + joint_names_.push_back("RHand"); + joint_names_.push_back("LHipYawPitch"); + joint_names_.push_back("RHipYawPitch"); + joint_names_.push_back("LHipRoll"); + joint_names_.push_back("LHipPitch"); + joint_names_.push_back("LKneePitch"); + joint_names_.push_back("LAnklePitch"); + joint_names_.push_back("LAnkleRoll"); + joint_names_.push_back("RHipRoll"); + joint_names_.push_back("RHipPitch"); + joint_names_.push_back("RKneePitch"); + joint_names_.push_back("RAnklePitch"); + joint_names_.push_back("RAnkleRoll"); + for(vector::iterator it=joint_names_.begin();it!=joint_names_.end();it++) + { + if((*it=="RHand" || *it=="LHand" || *it == "RWristYaw" || *it == "LWristYaw") && (body_type_ == "H21")) + { + joint_names_.erase(it); + it--; + continue; + } + joints_names_.arrayPush("Device/SubDeviceList/"+(*it)+"/Position/Sensor/Value"); + if(*it!="RHipYawPitch") + { + joint_temperature_names_.arrayPush("Device/SubDeviceList/"+(*it)+"/Temperature/Sensor/Value"); + } + } + number_of_joints_ = joint_names_.size(); + + // DCM Motion Commands Initialization + try + { + // Create Motion Command + commands_.arraySetSize(4); + commands_[0] = string("Joints"); + commands_[1] = string("ClearAll"); + commands_[2] = string("time-mixed"); + commands_[3].arraySetSize(number_of_joints_); + + // Create Joints Actuators Alias + AL::ALValue commandAlias; + commandAlias.arraySetSize(2); + commandAlias[0] = string("Joints"); + commandAlias[1].arraySetSize(number_of_joints_); + for(int i=0;i stiff = vector(number_of_joints_,1.0f); + vector times = vector(number_of_joints_,0); + DCMAliasTimedCommand("JointsHardness",stiff, times); + stiffnesses_enabled_ = true; + + // Add diagnostic functions + diagnostic_.setHardwareID(string("Nao")+version_+body_type_); + diagnostic_.add("Joints Temperature", this, &Nao::checkTemperature); + diagnostic_.add("Battery", this, &Nao::checkBattery); + + return true; +} + +bool Nao::initializeControllers(controller_manager::ControllerManager& cm) +{ + if(!initialize()) + { + ROS_ERROR("Initialization method failed!"); + return false; + } + + // Initialize Controllers' Interfaces + joint_angles_.resize(number_of_joints_); + joint_velocities_.resize(number_of_joints_); + joint_efforts_.resize(number_of_joints_); + joint_commands_.resize(number_of_joints_); + + try + { + for(int i=0;i(prefix_+"imu_data", topic_queue_); + + sonar_left_pub_ = node_handle_.advertise(prefix_+"sonar_left", topic_queue_); + sonar_left_.header.frame_id = "SonarLeft"; + sonar_left_.radiation_type = sensor_msgs::Range::ULTRASOUND; + sonar_left_.field_of_view = 1.04719755f; + sonar_left_.min_range = 0.25; + sonar_left_.max_range = 2.55; + + sonar_right_pub_ = node_handle_.advertise(prefix_+"sonar_right", topic_queue_); + sonar_right_.header.frame_id = "SonarRight"; + sonar_right_.radiation_type = sensor_msgs::Range::ULTRASOUND; + sonar_right_.field_of_view = 1.04719755f; + sonar_right_.min_range = 0.25; + sonar_right_.max_range = 2.55; + + sonar_switch_ = node_handle_.advertiseService(prefix_+"Sonar/Enable", &Nao::switchSonar, this); + + fsrs_pub_ = node_handle_.advertise(prefix_+"fsrs", topic_queue_); + + fsrs_switch_ = node_handle_.advertiseService(prefix_+"FSRs/Enable", &Nao::switchFSR, this); + + bumpers_pub_ = node_handle_.advertise(prefix_+"bumpers", topic_queue_); + + bumpers_switch_ = node_handle_.advertiseService(prefix_+"Bumpers/Enable", &Nao::switchBumper, this); + + tactiles_pub_ = node_handle_.advertise(prefix_+"tactiles", topic_queue_); + + tactiles_switch_ = node_handle_.advertiseService(prefix_+"Tactiles/Enable", &Nao::switchTactile, this); + + stiffness_pub_ = node_handle_.advertise(prefix_+"stiffnesses", topic_queue_); + stiffness_.data = 1.0f; + + stiffness_switch_ = node_handle_.advertiseService(prefix_+"Stiffnesses/Enable", &Nao::switchStiffnesses, this); +} + +void Nao::loadParams() +{ + // Load Server Parameters + node_handle_.param("Nao_Version", version_, string("V4")); + node_handle_.param("Nao_BodyType", body_type_, string("H21")); + + node_handle_.param("Nao_TactilesEnabled", tactiles_enabled_, true); + node_handle_.param("Nao_BumpersEnabled", bumpers_enabled_, true); + node_handle_.param("Nao_SonarEnabled", sonar_enabled_, true); + node_handle_.param("Nao_FootContactsEnabled", foot_contacts_enabled_, true); + + node_handle_.param("Nao_PublishIMU", imu_published_, true); + + node_handle_.param("Nao_TopicQueue", topic_queue_, 50); + + node_handle_.param("Nao_Prefix", prefix_, string("nao_dcm")); + prefix_ = prefix_+"/"; + + node_handle_.param("Nao_LowCommunicationFrequency", low_freq_, 10.0); + node_handle_.param("Nao_HighCommunicationFrequency", high_freq_, 100.0); + node_handle_.param("Nao_ControllerFrequency", controller_freq_, 15.0); +} + +void Nao::brokerDisconnected(const string& event_name, const string &broker_name, const string& subscriber_identifier) +{ + if(broker_name == "Nao Driver Broker") + is_connected_ = false; +} + +void Nao::DCMTimedCommand(const string &key, const AL::ALValue &value, const int &timeOffset, const string &type) +{ + try + { + // Create timed-command + AL::ALValue command; + command.arraySetSize(3); + command[0] = key; + command[1] = type; + command[2].arraySetSize(1); + command[2][0].arraySetSize(2); + command[2][0][0] = value; + command[2][0][1] = dcm_proxy_.getTime(timeOffset); + + // Execute timed-command + dcm_proxy_.set(command); + } + catch(const AL::ALError& e) + { + ROS_ERROR("Could not execute DCM timed-command!\n\t%s\n\n\tTrace: %s", key.c_str(), e.what()); + } +} + +void Nao::DCMAliasTimedCommand(const string &alias, const vector &values, const vector &timeOffsets, + const string &type, const string &type2) +{ + try + { + // Create Alias timed-command + AL::ALValue command; + command.arraySetSize(4); + command[0] = alias; + command[1] = type; + command[2] = type2; + command[3].arraySetSize(values.size()); + int T = dcm_proxy_.getTime(0); + for(int i=0;iupdate(time,ros::Duration(1.0f/controller_freq_)); + + writeJoints(); + + rate.sleep(); + } +} + +bool Nao::connected() +{ + return is_connected_; +} + +void Nao::commandVelocity(const geometry_msgs::TwistConstPtr &msg) +{ + ROS_WARN("This function does nothing at the moment.."); +} + +void Nao::publishIMU(const ros::Time &ts) +{ + vector memData; + try + { + memData = memory_proxy_.getListData(imu_names_); + } + catch(const AL::ALError& e) + { + ROS_ERROR("Could not get IMU data from Nao.\n\tTrace: %s",e.what()); + return; + } + + if (memData.size() != imu_names_.getSize()) + { + ROS_ERROR("IMU readings' size is not correct!"); + return; + } + + imu_.header.stamp = ts; + imu_.header.frame_id = "torso"; + + float angleX = memData[0]; + float angleY = memData[1]; + float angleZ = memData[2]; + float gyroX = memData[3]; + float gyroY = memData[4]; + float gyroZ = memData[5]; + float accX = memData[6]; + float accY = memData[7]; + float accZ = memData[8]; + + imu_.orientation = tf::createQuaternionMsgFromRollPitchYaw(angleX,angleY,angleZ); + + imu_.angular_velocity.x = gyroX; + imu_.angular_velocity.y = gyroY; + imu_.angular_velocity.z = gyroZ; + + imu_.linear_acceleration.x = accX; + imu_.linear_acceleration.y = accY; + imu_.linear_acceleration.z = accZ; + + // covariances unknown + imu_.orientation_covariance[0] = 0; + imu_.angular_velocity_covariance[0] = 0; + imu_.linear_acceleration_covariance[0] = 0; + + imu_pub_.publish(imu_); +} + +void Nao::publishBaseFootprint(const ros::Time &ts) +{ + string odom_frame, l_sole_frame, r_sole_frame, base_link_frame; + try { + odom_frame = base_footprint_listener_.resolve("odom_combined"); + l_sole_frame = base_footprint_listener_.resolve("l_sole"); + r_sole_frame = base_footprint_listener_.resolve("r_sole"); + base_link_frame = base_footprint_listener_.resolve("base_link"); + } + catch(ros::Exception& e) + { + ROS_ERROR("%s",e.what()); + return; + } + + tf::StampedTransform tf_odom_to_base, tf_odom_to_left_foot, tf_odom_to_right_foot; + double temp_freq = 1.0f/(10.0*high_freq_); + if(!base_footprint_listener_.waitForTransform(odom_frame, l_sole_frame, ros::Time(0), ros::Duration(temp_freq))) + return; + try { + base_footprint_listener_.lookupTransform(odom_frame, l_sole_frame, ros::Time(0), tf_odom_to_left_foot); + base_footprint_listener_.lookupTransform(odom_frame, r_sole_frame, ros::Time(0), tf_odom_to_right_foot); + base_footprint_listener_.lookupTransform(odom_frame, base_link_frame, ros::Time(0), tf_odom_to_base); + } + catch (const tf::TransformException& ex){ + ROS_ERROR("%s",ex.what()); + return ; + } + + tf::Vector3 new_origin = (tf_odom_to_right_foot.getOrigin() + tf_odom_to_left_foot.getOrigin())/2.0; + double height = std::min(tf_odom_to_left_foot.getOrigin().getZ(), tf_odom_to_right_foot.getOrigin().getZ()); + new_origin.setZ(height); + + double roll, pitch, yaw; + tf_odom_to_base.getBasis().getRPY(roll, pitch, yaw); + + tf::Transform tf_odom_to_footprint(tf::createQuaternionFromYaw(yaw), new_origin); + tf::Transform tf_base_to_footprint = tf_odom_to_base.inverse() * tf_odom_to_footprint; + + base_footprint_broadcaster_.sendTransform(tf::StampedTransform(tf_base_to_footprint, ts, + base_link_frame, "base_footprint")); +} + +void Nao::readJoints() +{ + vector jointData; + try + { + jointData = memory_proxy_.getListData(joints_names_); + } + catch(const AL::ALError& e) + { + ROS_ERROR("Could not get joint data from Nao.\n\tTrace: %s",e.what()); + return; + } + + for(short i = 0; i(number_of_joints_,1.0f), vector(number_of_joints_,0)); + } + else if(stiffnesses_enabled_!=req.enable) + { + DCMAliasTimedCommand("JointsHardness",vector(number_of_joints_,0.0f), vector(number_of_joints_,0)); + } + stiffnesses_enabled_ = req.enable; +} + +void Nao::checkSonar() +{ + // Send Sonar Wave + DCMTimedCommand("Device/SubDeviceList/US/Actuator/Value",4.0f,0); + + // Read Sonar Values + AL::ALValue sonars; + try + { + sonars = memory_proxy_.getListData(sonar_names_); + } + catch(const AL::ALError& e) + { + ROS_ERROR("Could not get sonar values.\n\tTrace: %s",e.what()); + return; + } + + // Select closer object detected + float sonar_left = float(sonars[0]), sonar_right = float(sonars[10]); + for(short i=1;i<10;i++) + { + if(float(sonars[i])>=0.0f && float(sonars[i])<=2.55f && float(sonars[i])=0.0f && float(sonars[10+i])<=2.55f && float(sonars[10+i])=70.0f) + stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Joints Temperature: WARNING!"); + if(float(temps[i])>=85.0f) + stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Joints Temperature: CRITICAL!"); + string joint_name = string(joint_temperature_names_[i]).erase(0,21); + short l = joint_name.find('/',joint_name.find('/')); + joint_name.erase(l,25); + stat.add(joint_name,temps[i]); + } +} + +void Nao::checkBattery(diagnostic_updater::DiagnosticStatusWrapper &stat) +{ + AL::ALValue batt; + try + { + batt = memory_proxy_.getListData(battery_names_); + } + catch(const AL::ALError& e) + { + ROS_ERROR("Could not get Battery values.\n\tTrace: %s",e.what()); + return; + } + int status = 0; + string message = "Battery: "+boost::lexical_cast(float(batt[0])*100.0f)+"\% charged! "; + if(float(batt[0])*100.0f<50.0f) + status = 1; + else if(float(batt[0])*100.0f<20.0f) + status = 2; + + if(float(batt[1])>=60.0f) + { + status = 1; + message += "Temperature: WARNING!"; + } + else if(float(batt[1])>=70.0f) + { + status = 2; + message += "Temperature: CRITICAL!"; + } + else + { + message += "Temperature: OK!"; + } + stat.summary(status,message); + + stat.add("Battery Charge",float(batt[0])*100.0f); + stat.add("Battery Temperature", float(batt[1])); +} diff --git a/nao_dcm_robot/nao_dcm_driver/src/nao_driver.cpp b/nao_dcm_robot/nao_dcm_driver/src/nao_driver.cpp new file mode 100644 index 0000000..2da7150 --- /dev/null +++ b/nao_dcm_robot/nao_dcm_driver/src/nao_driver.cpp @@ -0,0 +1,93 @@ +/**Copyright (c) 2014, Konstantinos Chatzilygeroudis +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. 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. + +3. Neither the name of the copyright holder 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 HOLDER 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. +**/ + +#include +#include "nao_dcm_driver/nao.h" +#include +#include + +using std::string; +using std::cerr; +using std::endl; + +int main( int argc, char** argv ) +{ + int pport = 9559; + string pip = "127.0.0.1"; + ros::init(argc, argv, "nao_dcm_driver"); + ros::NodeHandle n; + if(!ros::master::check()) + { + cerr<<"Could not contact master!\nQuitting... "< broker; + try + { + broker = AL::ALBroker::createBroker(broker_name,broker_ip,broker_port,pip,pport,0); + } + catch(...) + { + ROS_ERROR("Failed to connect to Broker at %s:%d!",pip.c_str(),pport); + return -1; + } + + // Deal with ALBrokerManager singleton (add your broker into NAOqi) + AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock()); + AL::ALBrokerManager::getInstance()->addBroker(broker); + + // Now it's time to load your module + boost::shared_ptr nao = AL::ALModule::createModule(broker, "Nao"); + nao->connect(n); + if(!nao->connected()) + { + ROS_ERROR("Could not connect to Nao robot!"); + AL::ALBrokerManager::getInstance()->killAllBroker(); + AL::ALBrokerManager::kill(); + return -1; + } + // Run the spinner in a separate thread to prevent lockups + ros::AsyncSpinner spinner(1); + spinner.start(); + if(broker->isModulePresent("Nao")) + ROS_INFO("Nao Module loaded succesfully!"); + else + { + ROS_ERROR("Nao Module is not loaded!"); + return -1; + } + + // Run Nao Driver Loop + nao->run(); + + AL::ALBrokerManager::getInstance()->killAllBroker(); + AL::ALBrokerManager::kill(); + spinner.stop(); + ROS_INFO( "Quitting... " ); + return 0; +} diff --git a/nao_dcm_robot/nao_dcm_msgs/CMakeLists.txt b/nao_dcm_robot/nao_dcm_msgs/CMakeLists.txt new file mode 100644 index 0000000..7d6e04b --- /dev/null +++ b/nao_dcm_robot/nao_dcm_msgs/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 2.8.3) +project(nao_dcm_msgs) + +#List to make rest of code more readable +set( MESSAGE_DEPENDENCIES + std_msgs +) + +#Declare build dependencies +find_package(catkin REQUIRED + COMPONENTS + message_generation + ${MESSAGE_DEPENDENCIES} ) + +add_message_files(DIRECTORY msg + FILES + FSRs.msg + Bumper.msg + Tactile.msg +) + +add_service_files(DIRECTORY srv + FILES + BoolService.srv +) + +#And now generate the messages +generate_messages(DEPENDENCIES ${MESSAGE_DEPENDENCIES}) + +#Declare package run-time dependencies +catkin_package( CATKIN_DEPENDS message_runtime ${MESSAGE_DEPENDENCIES}) \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_msgs/msg/Bumper.msg b/nao_dcm_robot/nao_dcm_msgs/msg/Bumper.msg new file mode 100644 index 0000000..e5e8e5a --- /dev/null +++ b/nao_dcm_robot/nao_dcm_msgs/msg/Bumper.msg @@ -0,0 +1,8 @@ +uint8 stateUnPressed=0 +uint8 statePressed=1 + +uint8 LeftFootLeft +uint8 LeftFootRight + +uint8 RightFootLeft +uint8 RightFootRight \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_msgs/msg/FSRs.msg b/nao_dcm_robot/nao_dcm_msgs/msg/FSRs.msg new file mode 100644 index 0000000..45ca09d --- /dev/null +++ b/nao_dcm_robot/nao_dcm_msgs/msg/FSRs.msg @@ -0,0 +1,17 @@ +float32 LeftFrontLeft +float32 LeftFrontRight +float32 LeftRearLeft +float32 LeftRearRight +float32 LeftTotalWeight + +float32 LeftCOPx +float32 LeftCOPy + +float32 RightFrontLeft +float32 RightFrontRight +float32 RightRearLeft +float32 RightRearRight +float32 RightTotalWeight + +float32 RightCOPx +float32 RightCOPy \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_msgs/msg/Tactile.msg b/nao_dcm_robot/nao_dcm_msgs/msg/Tactile.msg new file mode 100644 index 0000000..6ef342d --- /dev/null +++ b/nao_dcm_robot/nao_dcm_msgs/msg/Tactile.msg @@ -0,0 +1,14 @@ +uint8 stateUnPressed=0 +uint8 statePressed=1 + +uint8 HeadTouchRear +uint8 HeadTouchMiddle +uint8 HeadTouchFront + +uint8 LeftTouchBack +uint8 LeftTouchLeft +uint8 LeftTouchRight + +uint8 RightTouchBack +uint8 RightTouchLeft +uint8 RightTouchRight \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_msgs/package.xml b/nao_dcm_robot/nao_dcm_msgs/package.xml new file mode 100644 index 0000000..18ffec8 --- /dev/null +++ b/nao_dcm_robot/nao_dcm_msgs/package.xml @@ -0,0 +1,38 @@ + + + + nao_dcm_msgs + 0.0.1 + + Message, service and action declarations for Aldebaran's Nao (v4) + + + Konstantinos Chatzilygeroudis + + BSD + + https://github.com/costashatz/nao_dcm + https://github.com/costashatz/nao_dcm/issues + + Konstantinos Chatzilygeroudis + + catkin + + message_generation + std_msgs + + message_runtime + std_msgs + + \ No newline at end of file diff --git a/nao_dcm_robot/nao_dcm_msgs/srv/BoolService.srv b/nao_dcm_robot/nao_dcm_msgs/srv/BoolService.srv new file mode 100644 index 0000000..d0f476b --- /dev/null +++ b/nao_dcm_robot/nao_dcm_msgs/srv/BoolService.srv @@ -0,0 +1,2 @@ +bool enable +---