├── .gitignore ├── nao_dcm_robot ├── nao_dcm_msgs │ ├── srv │ │ └── BoolService.srv │ ├── msg │ │ ├── Bumper.msg │ │ ├── Tactile.msg │ │ └── FSRs.msg │ ├── CMakeLists.txt │ └── package.xml ├── nao_dcm_bringup │ ├── launch │ │ ├── nao_slam.launch │ │ ├── nao_dcm_bringup.launch │ │ ├── nao_dcm_odom_publisher.launch │ │ └── xtion_openni.launch │ ├── CMakeLists.txt │ ├── config │ │ └── nao_dcm.yaml │ └── package.xml └── nao_dcm_driver │ ├── config │ └── nao_dcm_analyzers.yaml │ ├── CMakeLists.txt │ ├── package.xml │ ├── src │ ├── nao_driver.cpp │ └── nao.cpp │ ├── cmake │ └── FindNAOqi.cmake │ └── include │ └── nao_dcm_driver │ └── nao.h ├── nao_dcm ├── CMakeLists.txt └── package.xml ├── LICENSE.txt └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | nao_dcm_driver-build 2 | CMakeLists.txt.user 3 | *.pyc -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_msgs/srv/BoolService.srv: -------------------------------------------------------------------------------- 1 | bool enable 2 | --- 3 | -------------------------------------------------------------------------------- /nao_dcm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(nao_dcm) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_bringup/launch/nao_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_msgs/msg/Bumper.msg: -------------------------------------------------------------------------------- 1 | uint8 stateUnPressed=0 2 | uint8 statePressed=1 3 | 4 | uint8 LeftFootLeft 5 | uint8 LeftFootRight 6 | 7 | uint8 RightFootLeft 8 | uint8 RightFootRight -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_msgs/msg/Tactile.msg: -------------------------------------------------------------------------------- 1 | uint8 stateUnPressed=0 2 | uint8 statePressed=1 3 | 4 | uint8 HeadTouchRear 5 | uint8 HeadTouchMiddle 6 | uint8 HeadTouchFront 7 | 8 | uint8 LeftTouchBack 9 | uint8 LeftTouchLeft 10 | uint8 LeftTouchRight 11 | 12 | uint8 RightTouchBack 13 | uint8 RightTouchLeft 14 | uint8 RightTouchRight -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_msgs/msg/FSRs.msg: -------------------------------------------------------------------------------- 1 | float32 LeftFrontLeft 2 | float32 LeftFrontRight 3 | float32 LeftRearLeft 4 | float32 LeftRearRight 5 | float32 LeftTotalWeight 6 | 7 | float32 LeftCOPx 8 | float32 LeftCOPy 9 | 10 | float32 RightFrontLeft 11 | float32 RightFrontRight 12 | float32 RightRearLeft 13 | float32 RightRearRight 14 | float32 RightTotalWeight 15 | 16 | float32 RightCOPx 17 | float32 RightCOPy -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(nao_dcm_bringup) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED) 6 | find_package(Boost) 7 | 8 | ##Needed for ros packages 9 | catkin_package(CATKIN_DEPENDS rospy nao_dcm_driver) 10 | 11 | install(DIRECTORY launch 12 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 13 | 14 | install(DIRECTORY config 15 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 16 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_driver/config/nao_dcm_analyzers.yaml: -------------------------------------------------------------------------------- 1 | pub_rate: 1.0 # Optional 2 | base_path: "" # Optional 3 | analyzers: 4 | nao_dcm_driver: 5 | type: AnalyzerGroup 6 | path: Nao 7 | analyzers: 8 | battery: 9 | type: GenericAnalyzer 10 | path: Battery 11 | remove_prefix: nao_dcm_driver 12 | contains: Battery 13 | joints: 14 | type: GenericAnalyzer 15 | path: Joints 16 | remove_prefix: nao_dcm_driver 17 | contains: Joints 18 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(nao_dcm_msgs) 3 | 4 | #List to make rest of code more readable 5 | set( MESSAGE_DEPENDENCIES 6 | std_msgs 7 | ) 8 | 9 | #Declare build dependencies 10 | find_package(catkin REQUIRED 11 | COMPONENTS 12 | message_generation 13 | ${MESSAGE_DEPENDENCIES} ) 14 | 15 | add_message_files(DIRECTORY msg 16 | FILES 17 | FSRs.msg 18 | Bumper.msg 19 | Tactile.msg 20 | ) 21 | 22 | add_service_files(DIRECTORY srv 23 | FILES 24 | BoolService.srv 25 | ) 26 | 27 | #And now generate the messages 28 | generate_messages(DEPENDENCIES ${MESSAGE_DEPENDENCIES}) 29 | 30 | #Declare package run-time dependencies 31 | catkin_package( CATKIN_DEPENDS message_runtime ${MESSAGE_DEPENDENCIES}) -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_bringup/config/nao_dcm.yaml: -------------------------------------------------------------------------------- 1 | # DO NOT CHANGE IT 2 | Version: V4 3 | 4 | # Choose what components are enabled on startup 5 | SonarEnabled: true 6 | TactilesEnabled: true 7 | BumpersEnabled: true 8 | FootContactsEnabled: true 9 | 10 | # Camera related parameters 11 | UseCamera: true 12 | TopCameraEnabled: true 13 | BottomCameraEnabled: true 14 | 15 | # Application related parameters 16 | PublishIMU: true 17 | 18 | # ROS related paramaters 19 | TopicQueue: 10 20 | 21 | # Robot-Connection related parameters 22 | RobotPort: 9559 23 | RobotIP: nao.local 24 | DriverBrokerPort: 54000 25 | DriverBrokerIP: 0.0.0.0 26 | CameraBrokerPort: 54001 27 | CameraBrokerIP: 0.0.0.0 28 | 29 | # Communication related parameters 30 | HighCommunicationFrequency: 15 31 | LowCommunicationFrequency: 10 32 | # 100Hz are perfect for Webots, 5-10Hz works really beautiful on my real Nao 33 | ControllerFrequency: 10 34 | CameraFrequency: 15 35 | 36 | # Robot related parameters 37 | JointPrecision: 0.00174532925 38 | OdomFrame: odom 39 | 40 | # DO NOT CHANGE IT 41 | Prefix: nao_dcm 42 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_bringup/launch/nao_dcm_odom_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014-2015, Konstantinos Chatzilygeroudis 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | 8 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 9 | in the documentation and/or other materials provided with the distribution. 10 | 11 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 12 | from this software without specific prior written permission. 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 15 | BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 16 | SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 17 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 18 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 19 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 20 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(nao_dcm_driver) 3 | 4 | # Tell CMake where to find "FindNAOqi.cmake" 5 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") 6 | 7 | ## Find catkin macros and libraries 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | rospy 11 | std_msgs 12 | geometry_msgs 13 | tf 14 | hardware_interface 15 | controller_manager 16 | nao_dcm_msgs 17 | sensor_msgs 18 | ) 19 | 20 | find_package(NAOqi QUIET COMPONENTS 21 | alcommon 22 | alvision 23 | alproxies 24 | alerror 25 | alvalue 26 | qi 27 | qitype 28 | ) 29 | 30 | find_package(Boost) 31 | 32 | ##Needed for ros packages 33 | catkin_package(CATKIN_DEPENDS roscpp geometry_msgs tf std_msgs nao_dcm_msgs sensor_msgs hardware_interface controller_manager) 34 | 35 | if( NAOqi_FOUND AND Boost_FOUND) 36 | message(STATUS "NAOqi C++ SDK found; C++ nodes will be built") 37 | include_directories( include 38 | ${catkin_INCLUDE_DIRS} 39 | ${NAOqi_INCLUDE_DIRS} 40 | ${Boost_INCLUDE_DIRS}) 41 | add_executable(nao_dcm_driver src/nao_driver.cpp src/nao.cpp) 42 | target_link_libraries(nao_dcm_driver 43 | ${catkin_LIBRARIES} 44 | ${NAOqi_LIBRARIES} 45 | ${Boost_LIBRARIES}) 46 | add_dependencies(nao_dcm_driver ${catkin_EXPORTED_TARGETS}) 47 | else() 48 | message(STATUS "Cannot find NAOqi C++ sdk; C++ nodes will NOT be built") 49 | endif() 50 | 51 | 52 | install(TARGETS nao_dcm_driver 53 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 54 | ) 55 | 56 | # install(DIRECTORY launch 57 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 58 | 59 | install(DIRECTORY config 60 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 61 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_bringup/launch/xtion_openni.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 21 | 22 | nao_dcm_bringup 23 | 0.0.1 24 | Bring-up the nao_dcm driver to connect to Aldebaran's Nao robot (v4). 25 | 26 | Konstantinos Chatzilygeroudis 27 | 28 | 29 | BSD 30 | 31 | https://github.com/costashatz/nao_dcm 32 | https://github.com/costashatz/nao_dcm/issues 33 | 34 | Konstantinos Chatzilygeroudis 35 | 36 | 37 | catkin 38 | 39 | rospy 40 | nao_dcm_driver 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 21 | 22 | nao_dcm_msgs 23 | 0.0.1 24 | 25 | Message, service and action declarations for Aldebaran's Nao (v4) 26 | 27 | 28 | Konstantinos Chatzilygeroudis 29 | 30 | BSD 31 | 32 | https://github.com/costashatz/nao_dcm 33 | https://github.com/costashatz/nao_dcm/issues 34 | 35 | Konstantinos Chatzilygeroudis 36 | 37 | catkin 38 | 39 | message_generation 40 | std_msgs 41 | 42 | message_runtime 43 | std_msgs 44 | 45 | -------------------------------------------------------------------------------- /nao_dcm/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 21 | 22 | nao_dcm 23 | 0.0.1 24 | nao_dcm metapackage contains packages to integrate into ROS Aldebaran's Nao robot (v4). 25 | This metapackage integrates the underlying machine of Nao and not the API provided by Aldebaran. 26 | 27 | 28 | Konstantinos Chatzilygeroudis 29 | 30 | 31 | BSD 32 | 33 | https://github.com/costashatz/nao_dcm 34 | https://github.com/costashatz/nao_dcm/issues 35 | 36 | Konstantinos Chatzilygeroudis 37 | 38 | catkin 39 | 40 | nao_description 41 | nao_dcm_driver 42 | nao_dcm_msgs 43 | nao_control 44 | nao_gazebo 45 | nao_moveit_config 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 21 | 22 | nao_dcm_driver 23 | 0.0.1 24 | Package containing the hardware interface to connect to Aldebaran's Nao robot (v4). 25 | 26 | Konstantinos Chatzilygeroudis 27 | 28 | 29 | BSD 30 | 31 | https://github.com/costashatz/nao_dcm 32 | https://github.com/costashatz/nao_dcm/issues 33 | 34 | Konstantinos Chatzilygeroudis 35 | 36 | 37 | catkin 38 | 39 | roscpp 40 | rospy 41 | std_msgs 42 | controller_manager 43 | hardware_interface 44 | geometry_msgs 45 | tf 46 | nao_dcm_msgs 47 | sensor_msgs 48 | 49 | 50 | roscpp 51 | rospy 52 | std_msgs 53 | geometry_msgs 54 | controller_manager 55 | hardware_interface 56 | tf 57 | nao_dcm_msgs 58 | sensor_msgs 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | nao_dcm 2 | ======== 3 | 4 | ROS Stack for Aldebaran's Nao Humanoid 5 | --------------------------------------- 6 | 7 | - Goal is to **connect to the machine of Nao** and not the API provided by Aldebaran. 8 | - Ιt makes use only of the **DCM and Memory Proxies**. 9 | - Supports **V4.0/V5.0 robots (H21 and H25 Body Types)**. 10 | - **Written purely in C++** and uses the lastest *C++ SDK (2.1.2.17)*. 11 | - **Coupled with official packages** 12 | 13 | 14 | Version 15 | ---- 16 | 17 | 0.0.1 (Beta) 18 | 19 | Requirements 20 | ----------- 21 | 22 | nao_dcm requires several packages to be installed in order to work properly: 23 | 24 | * [ROS] - ROS **Hydro** 25 | * [NaoQi C++ SDK] - **Version 2.1.2.17** - Should work on *1.14.5* too. 26 | * [ROS MoveIt!] - Used for motion planning 27 | * [ROS Control] - **Version >=0.6.0** 28 | * [Webots for Nao] - The best? simulator so far [optional] 29 | * [Gazebo] - Work in progress but with satisfactory results (**Version >= 2.2.2 alongside gazebo-ros-pkgs >= 2.3.4 and [roboticsgroup_gazebo_plugins]**) [optional] 30 | * [Nao Robot] - A real working Nao is the best "simulator" you'll ever get!! **Version >= V4.0 and flashed OpenNao OS >= 2.1.2.17** 31 | 32 | Basic Usage 33 | -------------- 34 | 35 | ### Bringup nao_dcm driver 36 | ```sh 37 | roslaunch nao_dcm_bringup nao_dcm_bringup.launch 38 | ``` 39 | 40 | This will connect to Nao Robot and provide basic control over Nao. 41 | 42 | ### Webots Simulation 43 | 44 | 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.* 45 | 46 | Notes/Limitations 47 | ----------------- 48 | * **nao_dcm can be run locally**, *but you need to have built and installed ROS Hydro/Indigo on Nao as nao_dcm requires ros_controls packages* (available only on >= Hydro). Yet, running **nao_dcm** driver remotely is identical in performance as running it locally (if the connection is strong and persistent - Wi-Fi usage is not recommended). 49 | * Tutorials will become available as soon as possible. 50 | * 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. 51 | * *Integration for LED, IR and Audio hardware is not available and is not on my agenda*. So, **feel free to contribute in that direction**. 52 | * **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. So, **PRs for code improvement are welcomed.** 53 | 54 | Origin of the Name 55 | ------------------ 56 | 57 | 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. 58 | 59 | License 60 | ---- 61 | 62 | BSD 63 | 64 | 65 | Copyright (c) 2014-2015, **Konstantinos Chatzilygeroudis** 66 | 67 | [ros]: http://www.ros.org 68 | [naoqi c++ sdk]: https://community.aldebaran-robotics.com/doc/2-1/index.html 69 | [webots for nao]: https://community.aldebaran-robotics.com/doc/2-1/software/webots/webots_index.html 70 | [gazebo]: http://gazebosim.org/ 71 | [ros moveit!]: http://moveit.ros.org/ 72 | [nao robot]: http://www.aldebaran.com/en/humanoid-robot/nao-robot 73 | [ros control]: http://wiki.ros.org/ros_control 74 | [roboticsgroup_gazebo_plugins]: http://github.com/roboticsgroup/roboticsgroup_gazebo_plugins 75 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_driver/src/nao_driver.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2014-2015, Konstantinos Chatzilygeroudis 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 10 | in the documentation and/or other materials provided with the distribution. 11 | 12 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 13 | from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 16 | BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 17 | SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 19 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 20 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 21 | **/ 22 | 23 | #include 24 | #include "nao_dcm_driver/nao.h" 25 | #include 26 | #include 27 | 28 | using std::string; 29 | using std::cerr; 30 | using std::endl; 31 | 32 | int main( int argc, char** argv ) 33 | { 34 | int pport = 9559; 35 | string pip = "127.0.0.1"; 36 | ros::init(argc, argv, "nao_dcm_driver"); 37 | ros::NodeHandle n; 38 | ros::NodeHandle n_p("~"); 39 | if(!ros::master::check()) 40 | { 41 | cerr<<"Could not contact master!\nQuitting... "< broker; 59 | try 60 | { 61 | broker = AL::ALBroker::createBroker(broker_name,broker_ip,broker_port,pip,pport,0); 62 | 63 | // Workaround because stiffness does not work well via DCM 64 | AL::ALProxy tempMotion("ALMotion", pip, pport); 65 | tempMotion.callVoid("setStiffnesses","Body",1.0f); 66 | } 67 | catch(...) 68 | { 69 | ROS_ERROR("Failed to connect to Broker at %s:%d!",pip.c_str(),pport); 70 | return -1; 71 | } 72 | 73 | // Deal with ALBrokerManager singleton (add your broker into NAOqi) 74 | AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock()); 75 | AL::ALBrokerManager::getInstance()->addBroker(broker); 76 | 77 | // Now it's time to load your module 78 | boost::shared_ptr nao = AL::ALModule::createModule(broker, "Nao"); 79 | nao->connect(n); 80 | if(!nao->connected()) 81 | { 82 | ROS_ERROR("Could not connect to Nao robot!"); 83 | AL::ALBrokerManager::getInstance()->killAllBroker(); 84 | AL::ALBrokerManager::kill(); 85 | return -1; 86 | } 87 | // Run the spinner in a separate thread to prevent lockups 88 | ros::AsyncSpinner spinner(1); 89 | spinner.start(); 90 | if(broker->isModulePresent("Nao")) 91 | ROS_INFO("Nao Module loaded succesfully!"); 92 | else 93 | { 94 | ROS_ERROR("Nao Module is not loaded!"); 95 | return -1; 96 | } 97 | 98 | // Run Nao Driver Loop 99 | nao->run(); 100 | 101 | AL::ALBrokerManager::getInstance()->killAllBroker(); 102 | AL::ALBrokerManager::kill(); 103 | spinner.stop(); 104 | ROS_INFO( "Quitting... " ); 105 | return 0; 106 | } 107 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_driver/cmake/FindNAOqi.cmake: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013, Miguel Sarabia 2 | # Imperial College London 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # # Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # # Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # # Neither the name of the Imperial College London nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | # 28 | 29 | 30 | # - Try to find NAOqi 31 | # Once executed this script will define the following: 32 | # NAOqi_FOUND - NAOqi was succesfully found 33 | # NAOqi_INCLUDE_DIRS - NAOqi's include directories 34 | # NAOqi_LIBRARIES - NAOqi's libraries 35 | # NAOqi_DIR - Directory where NAOqi was found 36 | #------------------------------------------------------------------------------ 37 | # Users can set NAOqi_DIR to force CMake to look in a particular location, 38 | # setting the AL_DIR environment variable will have a similar effect. 39 | 40 | cmake_minimum_required(VERSION 2.8.3) 41 | 42 | #These are NAOqi's known components (ie. libraries) 43 | set(NAOqi_COMPONENTS 44 | alaudio 45 | albonjourdiscovery 46 | alextractor 47 | allog 48 | almodelutils 49 | alproject 50 | alresource 51 | altools 52 | alautomatictest 53 | alboxrary 54 | alfile 55 | almathinternal 56 | almotion 57 | alpythonbridge 58 | alserial 59 | altts 60 | albehaviorinfo 61 | alcommon 62 | allauncher 63 | almath 64 | almotionrecorder 65 | alpythontools 66 | alsoap 67 | alvalue 68 | albehavior 69 | alerror 70 | allogremote 71 | almemoryfastaccess 72 | alparammanager 73 | alremotecall 74 | althread 75 | alvision 76 | alproxies 77 | qi 78 | qitype 79 | ) 80 | 81 | 82 | #Set INCLUDE hints 83 | set(NAOqi_INCLUDE_HINTS 84 | "${NAOqi_DIR}/include" 85 | "$ENV{AL_DIR}/include" ) 86 | 87 | # Set LIBRARY hints 88 | set(NAOqi_LIBRARY_HINTS 89 | "${NAOqi_DIR}/lib" 90 | "$ENV{AL_DIR}/lib" ) 91 | 92 | # Find include directories 93 | find_path(NAOqi_INCLUDE_DIR alcommon/alproxy.h HINTS ${NAOqi_INCLUDE_HINTS} ) 94 | 95 | # Verify we know about all the components requested 96 | # and remove those we don't know about 97 | set(NAOqi_FILTERED_COMPONENTS ${NAOqi_FIND_COMPONENTS}) 98 | 99 | if ( NAOqi_FIND_COMPONENTS ) 100 | foreach(comp ${NAOqi_FIND_COMPONENTS}) 101 | list(FIND NAOqi_COMPONENTS ${comp} ${comp}_KNOWN) 102 | if (${comp}_KNOWN EQUAL -1) 103 | list(REMOVE_ITEM NAOqi_FILTERED_COMPONENTS ${comp}) 104 | message(STATUS "Unknown NAOqi component ${comp}") 105 | endif() 106 | endforeach() 107 | endif() 108 | 109 | list(LENGTH NAOqi_FILTERED_COMPONENTS NAOqi_NUMBER_OF_COMPONENTS) 110 | set(NAOqi_FOUND_COMPONENTS TRUE) 111 | 112 | # Look for components (ie. libraries) 113 | if( ${NAOqi_NUMBER_OF_COMPONENTS} ) 114 | foreach(comp ${NAOqi_FILTERED_COMPONENTS}) 115 | #Look for the actual library here 116 | find_library(${comp}_LIBRARY NAMES ${comp} HINTS ${NAOqi_LIBRARY_HINTS}) 117 | if ( ${${comp}_LIBRARY} STREQUAL ${comp}_LIBRARY-NOTFOUND) 118 | message(STATUS "Could not find NAOqi's ${comp}") 119 | set(NAOqi_FOUND_COMPONENTS FALSE) 120 | else() 121 | #If everything went well append this component to list of libraries 122 | list(APPEND NAOqi_LIBRARY ${${comp}_LIBRARY}) 123 | endif() 124 | endforeach() 125 | else() 126 | message(STATUS "No NAOqi components specified") 127 | endif() 128 | 129 | 130 | # Handle the QUIET and REQUIRED arguments 131 | include(FindPackageHandleStandardArgs) 132 | find_package_handle_standard_args( 133 | NAOqi #Package name 134 | DEFAULT_MSG 135 | # Variables required to evaluate as TRUE 136 | NAOqi_LIBRARY 137 | NAOqi_INCLUDE_DIR 138 | NAOqi_FOUND_COMPONENTS) 139 | 140 | # Copy the values of the advanced variables to the user-facing ones 141 | set(NAOqi_LIBRARIES ${NAOqi_LIBRARY} ) 142 | set(NAOqi_INCLUDE_DIRS ${NAOqi_INCLUDE_DIR} ) 143 | set(NAOqi_FOUND ${NAOQI_FOUND}) 144 | 145 | # If NAOqi was found, update NAOqi_DIR to show where it was found 146 | if ( NAOqi_FOUND ) 147 | get_filename_component(NAOqi_NEW_DIR "${NAOqi_INCLUDE_DIRS}/../" ABSOLUTE) 148 | endif() 149 | set(NAOqi_DIR ${NAOqi_NEW_DIR} CACHE FILEPATH "NAOqi root directory" FORCE) 150 | 151 | #Hide these variables 152 | mark_as_advanced(NAOqi_INCLUDE_DIR NAOqi_LIBRARY NAOQI_FOUND) -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_driver/include/nao_dcm_driver/nao.h: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2014-2015, Konstantinos Chatzilygeroudis 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 10 | in the documentation and/or other materials provided with the distribution. 11 | 12 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 13 | from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 16 | BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 17 | SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 19 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 20 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 21 | **/ 22 | 23 | #ifndef NAO_DCM_DRIVER_NAO_H 24 | #define NAO_DCM_DRIVER_NAO_H 25 | 26 | // Boost Headers 27 | #include 28 | #include 29 | #include 30 | 31 | // NAOqi Headers 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | // ROS Headers 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include 53 | #include 54 | #include 55 | 56 | #include 57 | #include 58 | #include 59 | 60 | #include 61 | 62 | #include 63 | 64 | using std::string; 65 | using std::vector; 66 | 67 | namespace AL 68 | { 69 | class ALBroker; 70 | } 71 | 72 | // Helper definition 73 | template 74 | T * end(T (&ra)[N]) { 75 | return ra + N; 76 | } 77 | 78 | class Nao : public AL::ALModule, public hardware_interface::RobotHW 79 | { 80 | private: 81 | // ROS Standard Variables 82 | ros::NodeHandle node_handle_; 83 | 84 | // ROS Topics/Messages 85 | ros::Subscriber cmd_vel_sub_; 86 | 87 | ros::Publisher imu_pub_; 88 | sensor_msgs::Imu imu_; 89 | 90 | tf::TransformBroadcaster base_footprint_broadcaster_; 91 | tf::TransformListener base_footprint_listener_; 92 | 93 | ros::Publisher sonar_left_pub_, sonar_right_pub_; 94 | sensor_msgs::Range sonar_left_, sonar_right_; 95 | ros::ServiceServer sonar_switch_; 96 | 97 | ros::Publisher fsrs_pub_; 98 | nao_dcm_msgs::FSRs fsrs_; 99 | ros::ServiceServer fsrs_switch_; 100 | 101 | ros::Publisher bumpers_pub_, tactiles_pub_; 102 | nao_dcm_msgs::Bumper bumpers_; 103 | nao_dcm_msgs::Tactile tactiles_; 104 | ros::ServiceServer bumpers_switch_, tactiles_switch_; 105 | 106 | ros::Publisher stiffness_pub_; 107 | std_msgs::Float32 stiffness_; 108 | ros::ServiceServer stiffness_switch_; 109 | 110 | controller_manager::ControllerManager* manager_; 111 | 112 | // ROS Diagnostics 113 | diagnostic_updater::Updater diagnostic_; 114 | 115 | // Member Variables 116 | AL::ALValue commands_; 117 | 118 | // Helper 119 | bool is_connected_; 120 | 121 | // Robot Parameters 122 | string version_, body_type_; 123 | bool sonar_enabled_, tactiles_enabled_, bumpers_enabled_, foot_contacts_enabled_; 124 | bool imu_published_, stiffnesses_enabled_; 125 | int topic_queue_; 126 | string prefix_, odom_frame_; 127 | double low_freq_, high_freq_, controller_freq_, joint_precision_; 128 | 129 | // AL Proxies 130 | AL::ALMemoryProxy memory_proxy_; 131 | AL::DCMProxy dcm_proxy_; 132 | 133 | // IMU 134 | vector imu_names_; 135 | // Sonars 136 | vector sonar_names_; 137 | // FSRs 138 | vector fsr_names_; 139 | // Tactile 140 | vector tactile_names_; 141 | // Bumper 142 | vector bumper_names_; 143 | // Joints 144 | vector joints_names_; 145 | vector joint_temperature_names_; 146 | // Battery 147 | vector battery_names_; 148 | // LEDs 149 | vector led_names_; 150 | 151 | // Joint States 152 | hardware_interface::JointStateInterface jnt_state_interface_; 153 | hardware_interface::PositionJointInterface jnt_pos_interface_; 154 | 155 | int number_of_joints_; 156 | vector joint_names_; 157 | vector joint_commands_; 158 | vector joint_angles_; 159 | vector joint_velocities_; 160 | vector joint_efforts_; 161 | public: 162 | // Constructor/Destructor 163 | Nao(boost::shared_ptr broker, const std::string& name); 164 | ~Nao(); 165 | 166 | bool initialize(); 167 | bool initializeControllers(controller_manager::ControllerManager& cm); 168 | 169 | // Connect/Disconnet to ALProxies 170 | bool connect(const ros::NodeHandle nh); 171 | void disconnect(); 172 | 173 | // Subscribe/Advertise to ROS Topics/Services 174 | void subscribe(); 175 | 176 | // Parameter Server 177 | void loadParams(); 178 | 179 | // Helper 180 | void brokerDisconnected(const string& event_name, const string &broker_name, const string& subscriber_identifier); 181 | 182 | // DCMProxy Wrapper Methods 183 | void DCMTimedCommand(const string& key, const AL::ALValue& value, const int& timeOffset, 184 | const string& type="Merge"); 185 | void DCMAliasTimedCommand(const string& alias, const vector& values, const vector& timeOffsets, 186 | const string& type="Merge", const string& type2="time-mixed"); 187 | 188 | // ALMemoryProxy Wrapper Methods 189 | void insertDataToMemory(const string& key, const AL::ALValue& value); 190 | AL::ALValue getDataFromMemory(const string& key); 191 | void subscribeToEvent(const std::string& name, const std::string& callback_module, 192 | const std::string& callback_method); 193 | void subscribeToMicroEvent(const std::string& name, const std::string& callback_module, 194 | const std::string& callback_method, const string& callback_message=""); 195 | void unsubscribeFromEvent(const string& name, const string& callback_module); 196 | void unsubscribeFromMicroEvent(const string& name, const string& callback_module); 197 | void raiseEvent(const string& name, const AL::ALValue& value); 198 | void raiseMicroEvent(const string& name, const AL::ALValue& value); 199 | void declareEvent(const string& name); 200 | 201 | // General Methods 202 | void controllerLoop(); 203 | void lowCommunicationLoop(); 204 | void highCommunicationLoop(); 205 | 206 | bool connected(); 207 | 208 | // ROS Callbacks/Related Methods 209 | void commandVelocity(const geometry_msgs::TwistConstPtr &msg); 210 | 211 | void publishIMU(const ros::Time &ts); 212 | 213 | void publishBaseFootprint(const ros::Time &ts); 214 | 215 | void readJoints(); 216 | 217 | void writeJoints(); 218 | 219 | bool switchSonar(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res); 220 | 221 | bool switchFSR(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res); 222 | 223 | bool switchBumper(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res); 224 | 225 | bool switchTactile(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res); 226 | 227 | bool switchStiffnesses(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res); 228 | 229 | void checkSonar(); 230 | 231 | void checkFSR(); 232 | 233 | void checkTactile(); 234 | 235 | void checkBumper(); 236 | 237 | void checkTemperature(diagnostic_updater::DiagnosticStatusWrapper &stat); 238 | 239 | void checkBattery(diagnostic_updater::DiagnosticStatusWrapper &stat); 240 | 241 | void run(); 242 | 243 | }; 244 | 245 | #endif // NAO_H 246 | -------------------------------------------------------------------------------- /nao_dcm_robot/nao_dcm_driver/src/nao.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2014-2015, Konstantinos Chatzilygeroudis 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 10 | in the documentation and/or other materials provided with the distribution. 11 | 12 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 13 | from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 16 | BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 17 | SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 19 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 20 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 21 | **/ 22 | 23 | #include 24 | #include "nao_dcm_driver/nao.h" 25 | #include 26 | #include 27 | #include 28 | 29 | Nao::Nao(boost::shared_ptr broker, const string &name) 30 | : AL::ALModule(broker,name),is_connected_(false) 31 | { 32 | setModuleDescription("Nao Robot Module"); 33 | 34 | functionName("brokerDisconnected", getName(), "Callback when broker disconnects!"); 35 | BIND_METHOD(Nao::brokerDisconnected); 36 | } 37 | 38 | Nao::~Nao() 39 | { 40 | if(is_connected_) 41 | disconnect(); 42 | } 43 | 44 | bool Nao::initialize() 45 | { 46 | // IMU Memory Keys 47 | const char* imu[] = {"Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value", 48 | "Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value", 49 | "Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value", 50 | "Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value", 51 | "Device/SubDeviceList/InertialSensor/GyroscopeY/Sensor/Value", 52 | "Device/SubDeviceList/InertialSensor/GyroscopeZ/Sensor/Value", 53 | "Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value", 54 | "Device/SubDeviceList/InertialSensor/AccelerometerY/Sensor/Value", 55 | "Device/SubDeviceList/InertialSensor/AccelerometerZ/Sensor/Value"}; 56 | imu_names_ = vector(imu, end(imu)); 57 | 58 | // Sonar Memory Keys 59 | const char* sonar[] = {"Device/SubDeviceList/US/Left/Sensor/Value", 60 | "Device/SubDeviceList/US/Left/Sensor/Value1", 61 | "Device/SubDeviceList/US/Left/Sensor/Value2", 62 | "Device/SubDeviceList/US/Left/Sensor/Value3", 63 | "Device/SubDeviceList/US/Left/Sensor/Value4", 64 | "Device/SubDeviceList/US/Left/Sensor/Value5", 65 | "Device/SubDeviceList/US/Left/Sensor/Value6", 66 | "Device/SubDeviceList/US/Left/Sensor/Value7", 67 | "Device/SubDeviceList/US/Left/Sensor/Value8", 68 | "Device/SubDeviceList/US/Left/Sensor/Value9", 69 | "Device/SubDeviceList/US/Right/Sensor/Value", 70 | "Device/SubDeviceList/US/Right/Sensor/Value1", 71 | "Device/SubDeviceList/US/Right/Sensor/Value2", 72 | "Device/SubDeviceList/US/Right/Sensor/Value3", 73 | "Device/SubDeviceList/US/Right/Sensor/Value4", 74 | "Device/SubDeviceList/US/Right/Sensor/Value5", 75 | "Device/SubDeviceList/US/Right/Sensor/Value6", 76 | "Device/SubDeviceList/US/Right/Sensor/Value7", 77 | "Device/SubDeviceList/US/Right/Sensor/Value8", 78 | "Device/SubDeviceList/US/Right/Sensor/Value9"}; 79 | sonar_names_ = vector(sonar, end(sonar)); 80 | 81 | // Foot Contact Memory Keys 82 | const char* fsr[] = {"Device/SubDeviceList/LFoot/FSR/FrontLeft/Sensor/Value", 83 | "Device/SubDeviceList/LFoot/FSR/FrontRight/Sensor/Value", 84 | "Device/SubDeviceList/LFoot/FSR/RearLeft/Sensor/Value", 85 | "Device/SubDeviceList/LFoot/FSR/RearRight/Sensor/Value", 86 | "Device/SubDeviceList/LFoot/FSR/TotalWeight/Sensor/Value", 87 | "Device/SubDeviceList/RFoot/FSR/FrontLeft/Sensor/Value", 88 | "Device/SubDeviceList/RFoot/FSR/FrontRight/Sensor/Value", 89 | "Device/SubDeviceList/RFoot/FSR/RearLeft/Sensor/Value", 90 | "Device/SubDeviceList/RFoot/FSR/RearRight/Sensor/Value", 91 | "Device/SubDeviceList/RFoot/FSR/TotalWeight/Sensor/Value", 92 | "Device/SubDeviceList/LFoot/FSR/CenterOfPressure/X/Sensor/Value", 93 | "Device/SubDeviceList/LFoot/FSR/CenterOfPressure/Y/Sensor/Value", 94 | "Device/SubDeviceList/RFoot/FSR/CenterOfPressure/X/Sensor/Value", 95 | "Device/SubDeviceList/RFoot/FSR/CenterOfPressure/Y/Sensor/Value"}; 96 | fsr_names_ = vector(fsr, end(fsr)); 97 | 98 | // Tactile Memory Keys 99 | const char* tactile[] = {"Device/SubDeviceList/Head/Touch/Front/Sensor/Value", 100 | "Device/SubDeviceList/Head/Touch/Middle/Sensor/Value", 101 | "Device/SubDeviceList/Head/Touch/Rear/Sensor/Value", 102 | "Device/SubDeviceList/LHand/Touch/Back/Sensor/Value", 103 | "Device/SubDeviceList/LHand/Touch/Left/Sensor/Value", 104 | "Device/SubDeviceList/LHand/Touch/Right/Sensor/Value", 105 | "Device/SubDeviceList/RHand/Touch/Back/Sensor/Value", 106 | "Device/SubDeviceList/RHand/Touch/Left/Sensor/Value", 107 | "Device/SubDeviceList/RHand/Touch/Right/Sensor/Value"}; 108 | tactile_names_ = vector(tactile, end(tactile)); 109 | 110 | // Bumper Memory Keys 111 | const char* bumper[] = {"Device/SubDeviceList/LFoot/Bumper/Left/Sensor/Value", 112 | "Device/SubDeviceList/LFoot/Bumper/Right/Sensor/Value", 113 | "Device/SubDeviceList/RFoot/Bumper/Left/Sensor/Value", 114 | "Device/SubDeviceList/RFoot/Bumper/Right/Sensor/Value"}; 115 | bumper_names_ = vector(bumper, end(bumper)); 116 | 117 | // Battery Memory Keys 118 | const char* battery[] = {"Device/SubDeviceList/Battery/Charge/Sensor/Value", 119 | "Device/SubDeviceList/Battery/Temperature/Sensor/Value"}; 120 | battery_names_ = vector(battery, end(battery)); 121 | 122 | // LED Memory Keys 123 | const char* led[] = {"Device/SubDeviceList/ChestBoard/Led/Blue/Actuator/Value", 124 | "Device/SubDeviceList/ChestBoard/Led/Green/Actuator/Value", 125 | "Device/SubDeviceList/ChestBoard/Led/Red/Actuator/Value", 126 | "Device/SubDeviceList/Ears/Led/Left/0Deg/Actuator/Value", 127 | "Device/SubDeviceList/Ears/Led/Left/108Deg/Actuator/Value", 128 | "Device/SubDeviceList/Ears/Led/Left/144Deg/Actuator/Value", 129 | "Device/SubDeviceList/Ears/Led/Left/180Deg/Actuator/Value", 130 | "Device/SubDeviceList/Ears/Led/Left/216Deg/Actuator/Value", 131 | "Device/SubDeviceList/Ears/Led/Left/252Deg/Actuator/Value", 132 | "Device/SubDeviceList/Ears/Led/Left/288Deg/Actuator/Value", 133 | "Device/SubDeviceList/Ears/Led/Left/324Deg/Actuator/Value", 134 | "Device/SubDeviceList/Ears/Led/Left/36Deg/Actuator/Value", 135 | "Device/SubDeviceList/Ears/Led/Left/72Deg/Actuator/Value", 136 | "Device/SubDeviceList/Ears/Led/Right/0Deg/Actuator/Value", 137 | "Device/SubDeviceList/Ears/Led/Right/108Deg/Actuator/Value", 138 | "Device/SubDeviceList/Ears/Led/Right/144Deg/Actuator/Value", 139 | "Device/SubDeviceList/Ears/Led/Right/180Deg/Actuator/Value", 140 | "Device/SubDeviceList/Ears/Led/Right/216Deg/Actuator/Value", 141 | "Device/SubDeviceList/Ears/Led/Right/252Deg/Actuator/Value", 142 | "Device/SubDeviceList/Ears/Led/Right/288Deg/Actuator/Value", 143 | "Device/SubDeviceList/Ears/Led/Right/324Deg/Actuator/Value", 144 | "Device/SubDeviceList/Ears/Led/Right/36Deg/Actuator/Value", 145 | "Device/SubDeviceList/Ears/Led/Right/72Deg/Actuator/Value", 146 | "Device/SubDeviceList/Face/Led/Blue/Left/0Deg/Actuator/Value", 147 | "Device/SubDeviceList/Face/Led/Blue/Left/135Deg/Actuator/Value", 148 | "Device/SubDeviceList/Face/Led/Blue/Left/180Deg/Actuator/Value", 149 | "Device/SubDeviceList/Face/Led/Blue/Left/225Deg/Actuator/Value", 150 | "Device/SubDeviceList/Face/Led/Blue/Left/270Deg/Actuator/Value", 151 | "Device/SubDeviceList/Face/Led/Blue/Left/315Deg/Actuator/Value", 152 | "Device/SubDeviceList/Face/Led/Blue/Left/45Deg/Actuator/Value", 153 | "Device/SubDeviceList/Face/Led/Blue/Left/90Deg/Actuator/Value", 154 | "Device/SubDeviceList/Face/Led/Blue/Right/0Deg/Actuator/Value", 155 | "Device/SubDeviceList/Face/Led/Blue/Right/135Deg/Actuator/Value", 156 | "Device/SubDeviceList/Face/Led/Blue/Right/180Deg/Actuator/Value", 157 | "Device/SubDeviceList/Face/Led/Blue/Right/225Deg/Actuator/Value", 158 | "Device/SubDeviceList/Face/Led/Blue/Right/270Deg/Actuator/Value", 159 | "Device/SubDeviceList/Face/Led/Blue/Right/315Deg/Actuator/Value", 160 | "Device/SubDeviceList/Face/Led/Blue/Right/45Deg/Actuator/Value", 161 | "Device/SubDeviceList/Face/Led/Blue/Right/90Deg/Actuator/Value", 162 | "Device/SubDeviceList/Face/Led/Green/Left/0Deg/Actuator/Value", 163 | "Device/SubDeviceList/Face/Led/Green/Left/135Deg/Actuator/Value", 164 | "Device/SubDeviceList/Face/Led/Green/Left/180Deg/Actuator/Value", 165 | "Device/SubDeviceList/Face/Led/Green/Left/225Deg/Actuator/Value", 166 | "Device/SubDeviceList/Face/Led/Green/Left/270Deg/Actuator/Value", 167 | "Device/SubDeviceList/Face/Led/Green/Left/315Deg/Actuator/Value", 168 | "Device/SubDeviceList/Face/Led/Green/Left/45Deg/Actuator/Value", 169 | "Device/SubDeviceList/Face/Led/Green/Left/90Deg/Actuator/Value", 170 | "Device/SubDeviceList/Face/Led/Green/Right/0Deg/Actuator/Value", 171 | "Device/SubDeviceList/Face/Led/Green/Right/135Deg/Actuator/Value", 172 | "Device/SubDeviceList/Face/Led/Green/Right/180Deg/Actuator/Value", 173 | "Device/SubDeviceList/Face/Led/Green/Right/225Deg/Actuator/Value", 174 | "Device/SubDeviceList/Face/Led/Green/Right/270Deg/Actuator/Value", 175 | "Device/SubDeviceList/Face/Led/Green/Right/315Deg/Actuator/Value", 176 | "Device/SubDeviceList/Face/Led/Green/Right/45Deg/Actuator/Value", 177 | "Device/SubDeviceList/Face/Led/Green/Right/90Deg/Actuator/Value", 178 | "Device/SubDeviceList/Face/Led/Red/Left/0Deg/Actuator/Value", 179 | "Device/SubDeviceList/Face/Led/Red/Left/135Deg/Actuator/Value", 180 | "Device/SubDeviceList/Face/Led/Red/Left/180Deg/Actuator/Value", 181 | "Device/SubDeviceList/Face/Led/Red/Left/225Deg/Actuator/Value", 182 | "Device/SubDeviceList/Face/Led/Red/Left/270Deg/Actuator/Value", 183 | "Device/SubDeviceList/Face/Led/Red/Left/315Deg/Actuator/Value", 184 | "Device/SubDeviceList/Face/Led/Red/Left/45Deg/Actuator/Value", 185 | "Device/SubDeviceList/Face/Led/Red/Left/90Deg/Actuator/Value", 186 | "Device/SubDeviceList/Face/Led/Red/Right/0Deg/Actuator/Value", 187 | "Device/SubDeviceList/Face/Led/Red/Right/135Deg/Actuator/Value", 188 | "Device/SubDeviceList/Face/Led/Red/Right/180Deg/Actuator/Value", 189 | "Device/SubDeviceList/Face/Led/Red/Right/225Deg/Actuator/Value", 190 | "Device/SubDeviceList/Face/Led/Red/Right/270Deg/Actuator/Value", 191 | "Device/SubDeviceList/Face/Led/Red/Right/315Deg/Actuator/Value", 192 | "Device/SubDeviceList/Face/Led/Red/Right/45Deg/Actuator/Value", 193 | "Device/SubDeviceList/Face/Led/Red/Right/90Deg/Actuator/Value", 194 | "Device/SubDeviceList/Head/Led/Front/Left/0/Actuator/Value", 195 | "Device/SubDeviceList/Head/Led/Front/Left/1/Actuator/Value", 196 | "Device/SubDeviceList/Head/Led/Front/Right/0/Actuator/Value", 197 | "Device/SubDeviceList/Head/Led/Front/Right/1/Actuator/Value", 198 | "Device/SubDeviceList/Head/Led/Middle/Left/0/Actuator/Value", 199 | "Device/SubDeviceList/Head/Led/Middle/Right/0/Actuator/Value", 200 | "Device/SubDeviceList/Head/Led/Rear/Left/0/Actuator/Value", 201 | "Device/SubDeviceList/Head/Led/Rear/Left/1/Actuator/Value", 202 | "Device/SubDeviceList/Head/Led/Rear/Left/2/Actuator/Value", 203 | "Device/SubDeviceList/Head/Led/Rear/Right/0/Actuator/Value", 204 | "Device/SubDeviceList/Head/Led/Rear/Right/1/Actuator/Value", 205 | "Device/SubDeviceList/Head/Led/Rear/Right/2/Actuator/Value", 206 | "Device/SubDeviceList/LFoot/Led/Blue/Actuator/Value", 207 | "Device/SubDeviceList/LFoot/Led/Green/Actuator/Value", 208 | "Device/SubDeviceList/LFoot/Led/Red/Actuator/Value", 209 | "Device/SubDeviceList/RFoot/Led/Blue/Actuator/Value", 210 | "Device/SubDeviceList/RFoot/Led/Green/Actuator/Value", 211 | "Device/SubDeviceList/RFoot/Led/Red/Actuator/Value"}; 212 | led_names_ = vector(led, end(led)); 213 | 214 | // Joints Initialization 215 | const char* joint[] = {"HeadYaw", 216 | "HeadPitch", 217 | "LShoulderPitch", 218 | "LShoulderRoll", 219 | "LElbowYaw", 220 | "LElbowRoll", 221 | "LWristYaw", 222 | "LHand", 223 | "RShoulderPitch", 224 | "RShoulderRoll", 225 | "RElbowYaw", 226 | "RElbowRoll", 227 | "RWristYaw", 228 | "RHand", 229 | "LHipYawPitch", 230 | "RHipYawPitch", 231 | "LHipRoll", 232 | "LHipPitch", 233 | "LKneePitch", 234 | "LAnklePitch", 235 | "LAnkleRoll", 236 | "RHipRoll", 237 | "RHipPitch", 238 | "RKneePitch", 239 | "RAnklePitch", 240 | "RAnkleRoll"}; 241 | joint_names_ = vector(joint, end(joint)); 242 | 243 | for(vector::iterator it=joint_names_.begin();it!=joint_names_.end();it++) 244 | { 245 | if((*it=="RHand" || *it=="LHand" || *it == "RWristYaw" || *it == "LWristYaw") && (body_type_ == "H21")) 246 | { 247 | joint_names_.erase(it); 248 | it--; 249 | continue; 250 | } 251 | joints_names_.push_back("Device/SubDeviceList/"+(*it)+"/Position/Sensor/Value"); 252 | if(*it!="RHipYawPitch") 253 | { 254 | joint_temperature_names_.push_back("Device/SubDeviceList/"+(*it)+"/Temperature/Sensor/Value"); 255 | } 256 | } 257 | number_of_joints_ = joint_names_.size(); 258 | 259 | // DCM Motion Commands Initialization 260 | try 261 | { 262 | // Create Motion Command 263 | commands_.arraySetSize(4); 264 | commands_[0] = string("Joints"); 265 | commands_[1] = string("ClearAll"); 266 | commands_[2] = string("time-mixed"); 267 | commands_[3].arraySetSize(number_of_joints_); 268 | 269 | // Create Joints Actuators Alias 270 | AL::ALValue commandAlias; 271 | commandAlias.arraySetSize(2); 272 | commandAlias[0] = string("Joints"); 273 | commandAlias[1].arraySetSize(number_of_joints_); 274 | for(int i=0;i stiff = vector(number_of_joints_-1,1.0f); 314 | vector times = vector(number_of_joints_-1,0); 315 | DCMAliasTimedCommand("JointsHardness",stiff, times); 316 | stiffnesses_enabled_ = true; 317 | 318 | // Add diagnostic functions 319 | diagnostic_.setHardwareID(string("Nao")+version_+body_type_); 320 | diagnostic_.add("Joints Temperature", this, &Nao::checkTemperature); 321 | diagnostic_.add("Battery", this, &Nao::checkBattery); 322 | 323 | return true; 324 | } 325 | 326 | bool Nao::initializeControllers(controller_manager::ControllerManager& cm) 327 | { 328 | if(!initialize()) 329 | { 330 | ROS_ERROR("Initialization method failed!"); 331 | return false; 332 | } 333 | 334 | // Initialize Controllers' Interfaces 335 | joint_angles_.resize(number_of_joints_); 336 | joint_velocities_.resize(number_of_joints_); 337 | joint_efforts_.resize(number_of_joints_); 338 | joint_commands_.resize(number_of_joints_); 339 | 340 | try 341 | { 342 | for(int i=0;i(prefix_+"imu_data", topic_queue_); 444 | 445 | sonar_left_pub_ = node_handle_.advertise(prefix_+"sonar_left", topic_queue_); 446 | sonar_left_.header.frame_id = "LSonar_frame"; 447 | sonar_left_.radiation_type = sensor_msgs::Range::ULTRASOUND; 448 | sonar_left_.field_of_view = 1.04719755f; 449 | sonar_left_.min_range = 0.25; 450 | sonar_left_.max_range = 2.55; 451 | 452 | sonar_right_pub_ = node_handle_.advertise(prefix_+"sonar_right", topic_queue_); 453 | sonar_right_.header.frame_id = "RSonar_frame"; 454 | sonar_right_.radiation_type = sensor_msgs::Range::ULTRASOUND; 455 | sonar_right_.field_of_view = 1.04719755f; 456 | sonar_right_.min_range = 0.25; 457 | sonar_right_.max_range = 2.55; 458 | 459 | sonar_switch_ = node_handle_.advertiseService(prefix_+"Sonar/Enable", &Nao::switchSonar, this); 461 | 462 | fsrs_pub_ = node_handle_.advertise(prefix_+"fsrs", topic_queue_); 463 | 464 | fsrs_switch_ = node_handle_.advertiseService(prefix_+"FSRs/Enable", &Nao::switchFSR, this); 466 | 467 | bumpers_pub_ = node_handle_.advertise(prefix_+"bumpers", topic_queue_); 468 | 469 | bumpers_switch_ = node_handle_.advertiseService(prefix_+"Bumpers/Enable", &Nao::switchBumper, this); 471 | 472 | tactiles_pub_ = node_handle_.advertise(prefix_+"tactiles", topic_queue_); 473 | 474 | tactiles_switch_ = node_handle_.advertiseService(prefix_+"Tactiles/Enable", &Nao::switchTactile, this); 476 | 477 | stiffness_pub_ = node_handle_.advertise(prefix_+"stiffnesses", topic_queue_); 478 | stiffness_.data = 1.0f; 479 | 480 | stiffness_switch_ = node_handle_.advertiseService(prefix_+"Stiffnesses/Enable", &Nao::switchStiffnesses, this); 482 | } 483 | 484 | void Nao::loadParams() 485 | { 486 | ros::NodeHandle n_p("~"); 487 | // Load Server Parameters 488 | n_p.param("Version", version_, string("V4")); 489 | n_p.param("BodyType", body_type_, string("H25")); 490 | 491 | n_p.param("TactilesEnabled", tactiles_enabled_, true); 492 | n_p.param("BumpersEnabled", bumpers_enabled_, true); 493 | n_p.param("SonarEnabled", sonar_enabled_, true); 494 | n_p.param("FootContactsEnabled", foot_contacts_enabled_, true); 495 | 496 | n_p.param("PublishIMU", imu_published_, true); 497 | 498 | n_p.param("TopicQueue", topic_queue_, 50); 499 | 500 | n_p.param("Prefix", prefix_, string("nao_dcm")); 501 | prefix_ = prefix_+"/"; 502 | 503 | n_p.param("LowCommunicationFrequency", low_freq_, 10.0); 504 | n_p.param("HighCommunicationFrequency", high_freq_, 100.0); 505 | n_p.param("ControllerFrequency", controller_freq_, 15.0); 506 | n_p.param("JointPrecision", joint_precision_, 0.00174532925); 507 | n_p.param("OdomFrame", odom_frame_, string("odom")); 508 | } 509 | 510 | void Nao::brokerDisconnected(const string& event_name, const string &broker_name, const string& subscriber_identifier) 511 | { 512 | if(broker_name == "Nao Driver Broker") 513 | is_connected_ = false; 514 | } 515 | 516 | void Nao::DCMTimedCommand(const string &key, const AL::ALValue &value, const int &timeOffset, const string &type) 517 | { 518 | try 519 | { 520 | // Create timed-command 521 | AL::ALValue command; 522 | command.arraySetSize(3); 523 | command[0] = key; 524 | command[1] = type; 525 | command[2].arraySetSize(1); 526 | command[2][0].arraySetSize(2); 527 | command[2][0][0] = value; 528 | command[2][0][1] = dcm_proxy_.getTime(timeOffset); 529 | 530 | // Execute timed-command 531 | dcm_proxy_.set(command); 532 | } 533 | catch(const AL::ALError& e) 534 | { 535 | ROS_ERROR("Could not execute DCM timed-command!\n\t%s\n\n\tTrace: %s", key.c_str(), e.what()); 536 | } 537 | } 538 | 539 | void Nao::DCMAliasTimedCommand(const string &alias, const vector &values, const vector &timeOffsets, 540 | const string &type, const string &type2) 541 | { 542 | try 543 | { 544 | // Create Alias timed-command 545 | AL::ALValue command; 546 | command.arraySetSize(4); 547 | command[0] = alias; 548 | command[1] = type; 549 | command[2] = type2; 550 | command[3].arraySetSize(values.size()); 551 | int T = dcm_proxy_.getTime(0); 552 | for(int i=0;iupdate(time,ros::Duration(1.0f/controller_freq_)); 753 | 754 | writeJoints(); 755 | 756 | rate.sleep(); 757 | } 758 | } 759 | 760 | bool Nao::connected() 761 | { 762 | return is_connected_; 763 | } 764 | 765 | void Nao::commandVelocity(const geometry_msgs::TwistConstPtr &msg) 766 | { 767 | ROS_WARN("This function does nothing at the moment.."); 768 | } 769 | 770 | void Nao::publishIMU(const ros::Time &ts) 771 | { 772 | vector memData; 773 | try 774 | { 775 | memData = memory_proxy_.getListData(imu_names_); 776 | } 777 | catch(const AL::ALError& e) 778 | { 779 | ROS_ERROR("Could not get IMU data from Nao.\n\tTrace: %s",e.what()); 780 | return; 781 | } 782 | 783 | if (memData.size() != imu_names_.size()) 784 | { 785 | ROS_ERROR("IMU readings' size is not correct!"); 786 | return; 787 | } 788 | 789 | imu_.header.stamp = ts; 790 | imu_.header.frame_id = "torso"; 791 | 792 | float angleX = memData[0]; 793 | float angleY = memData[1]; 794 | float angleZ = memData[2]; 795 | float gyroX = memData[3]; 796 | float gyroY = memData[4]; 797 | float gyroZ = memData[5]; 798 | float accX = memData[6]; 799 | float accY = memData[7]; 800 | float accZ = memData[8]; 801 | 802 | imu_.orientation = tf::createQuaternionMsgFromRollPitchYaw(angleX,angleY,angleZ); 803 | 804 | imu_.angular_velocity.x = gyroX; 805 | imu_.angular_velocity.y = gyroY; 806 | imu_.angular_velocity.z = gyroZ; 807 | 808 | imu_.linear_acceleration.x = accX; 809 | imu_.linear_acceleration.y = accY; 810 | imu_.linear_acceleration.z = accZ; 811 | 812 | // covariances unknown 813 | imu_.orientation_covariance[0] = 0; 814 | imu_.angular_velocity_covariance[0] = 0; 815 | imu_.linear_acceleration_covariance[0] = 0; 816 | 817 | imu_pub_.publish(imu_); 818 | } 819 | 820 | void Nao::publishBaseFootprint(const ros::Time &ts) 821 | { 822 | string odom_frame, l_sole_frame, r_sole_frame, base_link_frame; 823 | try { 824 | odom_frame = base_footprint_listener_.resolve(odom_frame_); 825 | l_sole_frame = base_footprint_listener_.resolve("l_sole"); 826 | r_sole_frame = base_footprint_listener_.resolve("r_sole"); 827 | base_link_frame = base_footprint_listener_.resolve("base_link"); 828 | } 829 | catch(ros::Exception& e) 830 | { 831 | ROS_ERROR("%s",e.what()); 832 | return; 833 | } 834 | 835 | tf::StampedTransform tf_odom_to_base, tf_odom_to_left_foot, tf_odom_to_right_foot; 836 | double temp_freq = 1.0f/(10.0*high_freq_); 837 | if(!base_footprint_listener_.waitForTransform(odom_frame, l_sole_frame, ros::Time(0), ros::Duration(temp_freq))) 838 | return; 839 | try { 840 | base_footprint_listener_.lookupTransform(odom_frame, l_sole_frame, ros::Time(0), tf_odom_to_left_foot); 841 | base_footprint_listener_.lookupTransform(odom_frame, r_sole_frame, ros::Time(0), tf_odom_to_right_foot); 842 | base_footprint_listener_.lookupTransform(odom_frame, base_link_frame, ros::Time(0), tf_odom_to_base); 843 | } 844 | catch (const tf::TransformException& ex){ 845 | ROS_ERROR("%s",ex.what()); 846 | return ; 847 | } 848 | 849 | tf::Vector3 new_origin = (tf_odom_to_right_foot.getOrigin() + tf_odom_to_left_foot.getOrigin())/2.0; 850 | double height = std::min(tf_odom_to_left_foot.getOrigin().getZ(), tf_odom_to_right_foot.getOrigin().getZ()); 851 | new_origin.setZ(height); 852 | 853 | double roll, pitch, yaw; 854 | tf_odom_to_base.getBasis().getRPY(roll, pitch, yaw); 855 | 856 | tf::Transform tf_odom_to_footprint(tf::createQuaternionFromYaw(yaw), new_origin); 857 | tf::Transform tf_base_to_footprint = tf_odom_to_base.inverse() * tf_odom_to_footprint; 858 | 859 | base_footprint_broadcaster_.sendTransform(tf::StampedTransform(tf_base_to_footprint, ts, 860 | base_link_frame, "base_footprint")); 861 | } 862 | 863 | void Nao::readJoints() 864 | { 865 | vector jointData; 866 | try 867 | { 868 | jointData = memory_proxy_.getListData(joints_names_); 869 | } 870 | catch(const AL::ALError& e) 871 | { 872 | ROS_ERROR("Could not get joint data from Nao.\n\tTrace: %s",e.what()); 873 | return; 874 | } 875 | 876 | for(short i = 0; ijoint_precision_) 891 | { 892 | changed = true; 893 | break; 894 | } 895 | } 896 | // Do not write joints if no change in joint values 897 | if(!changed) 898 | { 899 | return; 900 | } 901 | 902 | try 903 | { 904 | int T = dcm_proxy_.getTime(0); 905 | for(int i=0;i(number_of_joints_,1.0f), vector(number_of_joints_,0)); 949 | } 950 | else if(stiffnesses_enabled_!=req.enable) 951 | { 952 | DCMAliasTimedCommand("JointsHardness",vector(number_of_joints_,0.0f), vector(number_of_joints_,0)); 953 | } 954 | stiffnesses_enabled_ = req.enable; 955 | } 956 | 957 | void Nao::checkSonar() 958 | { 959 | // Send Sonar Wave 960 | DCMTimedCommand("Device/SubDeviceList/US/Actuator/Value",4.0f,0); 961 | 962 | // Read Sonar Values 963 | AL::ALValue sonars; 964 | try 965 | { 966 | sonars = memory_proxy_.getListData(sonar_names_); 967 | } 968 | catch(const AL::ALError& e) 969 | { 970 | ROS_ERROR("Could not get sonar values.\n\tTrace: %s",e.what()); 971 | return; 972 | } 973 | 974 | // Select closer object detected 975 | float sonar_left = float(sonars[0]), sonar_right = float(sonars[10]); 976 | for(short i=1;i<10;i++) 977 | { 978 | 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) 1095 | stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Joints Temperature: WARNING!"); 1096 | if(float(temps[i])>=85.0f) 1097 | stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Joints Temperature: CRITICAL!"); 1098 | string joint_name = string(joint_temperature_names_[i]).erase(0,21); 1099 | short l = joint_name.find('/',joint_name.find('/')); 1100 | joint_name.erase(l,25); 1101 | stat.add(joint_name,temps[i]); 1102 | } 1103 | } 1104 | 1105 | void Nao::checkBattery(diagnostic_updater::DiagnosticStatusWrapper &stat) 1106 | { 1107 | AL::ALValue batt; 1108 | try 1109 | { 1110 | batt = memory_proxy_.getListData(battery_names_); 1111 | } 1112 | catch(const AL::ALError& e) 1113 | { 1114 | ROS_ERROR("Could not get Battery values.\n\tTrace: %s",e.what()); 1115 | return; 1116 | } 1117 | int status = 0; 1118 | string message = "Battery: "+boost::lexical_cast(float(batt[0])*100.0f)+"\% charged! "; 1119 | if(float(batt[0])*100.0f<50.0f) 1120 | status = 1; 1121 | else if(float(batt[0])*100.0f<20.0f) 1122 | status = 2; 1123 | 1124 | if(float(batt[1])>=60.0f) 1125 | { 1126 | status = 1; 1127 | message += "Temperature: WARNING!"; 1128 | } 1129 | else if(float(batt[1])>=70.0f) 1130 | { 1131 | status = 2; 1132 | message += "Temperature: CRITICAL!"; 1133 | } 1134 | else 1135 | { 1136 | message += "Temperature: OK!"; 1137 | } 1138 | stat.summary(status,message); 1139 | 1140 | stat.add("Battery Charge",float(batt[0])*100.0f); 1141 | stat.add("Battery Temperature", float(batt[1])); 1142 | } 1143 | --------------------------------------------------------------------------------