From 2c8d64aac95e6c3ae5917013cd81dd9d6682df77 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Fri, 23 Dec 2016 10:44:58 -0500 Subject: [PATCH] Move packages into monorepo for kinetic; strip out ur packages --- .gitignore | 45 +- README.md | 29 +- husky_base/CHANGELOG.rst | 98 + husky_base/CMakeLists.txt | 56 + husky_base/config/diagnostics.yaml | 7 + .../husky_base/horizon_legacy/Exception.h | 68 + .../husky_base/horizon_legacy/Logger.h | 112 + .../husky_base/horizon_legacy/Message.h | 320 ++ .../husky_base/horizon_legacy/Message_cmd.h | 298 ++ .../husky_base/horizon_legacy/Message_data.h | 1090 ++++++ .../horizon_legacy/Message_request.h | 66 + .../husky_base/horizon_legacy/Number.h | 85 + .../husky_base/horizon_legacy/Transport.h | 180 + .../husky_base/horizon_legacy/clearpath.h | 55 + .../include/husky_base/horizon_legacy/crc.h | 60 + .../husky_base/horizon_legacy/serial.h | 61 + .../husky_base/horizon_legacy_wrapper.h | 122 + .../include/husky_base/husky_diagnostics.h | 108 + .../include/husky_base/husky_hardware.h | 117 + husky_base/launch/base.launch | 60 + husky_base/package.xml | 44 + husky_base/src/horizon_legacy/Logger.cpp | 154 + husky_base/src/horizon_legacy/Message.cpp | 502 +++ husky_base/src/horizon_legacy/Message_cmd.cpp | 377 ++ .../src/horizon_legacy/Message_data.cpp | 1150 ++++++ .../src/horizon_legacy/Message_request.cpp | 66 + husky_base/src/horizon_legacy/Number.cpp | 168 + husky_base/src/horizon_legacy/Transport.cpp | 696 ++++ husky_base/src/horizon_legacy/crc.cpp | 88 + .../src/horizon_legacy/linux_serial.cpp | 174 + husky_base/src/horizon_legacy_wrapper.cpp | 98 + husky_base/src/husky_base.cpp | 108 + husky_base/src/husky_diagnostics.cpp | 256 ++ husky_base/src/husky_hardware.cpp | 241 ++ husky_bringup/CHANGELOG.rst | 102 + husky_bringup/CMakeLists.txt | 16 + husky_bringup/config/mag_config_default.yaml | 4 + .../debian/ros-indigo-husky-bringup.udev | 2 + .../env-hooks/50.husky_find_mag_config.sh | 6 + .../launch/lms1xx_config/lms1xx.launch | 10 + .../microstrain_config/microstrain.launch | 13 + .../launch/navsat_config/navsat.launch | 25 + husky_bringup/launch/um6_config/um6.launch | 33 + husky_bringup/launch/um7_config/um7.launch | 33 + .../launch/ur5_arm_config/ur5.launch | 62 + husky_bringup/package.xml | 42 + husky_bringup/scripts/calibrate_compass | 68 + husky_bringup/scripts/compute_calibration | 84 + husky_bringup/scripts/install | 29 + husky_bringup/udev/10-microstrain.rules | 1 + husky_bringup/udev/41-clearpath.rules | 1 + husky_bringup/udev/41-hokuyo.rules | 1 + husky_bringup/udev/52-ftdi.rules | 1 + husky_bringup/udev/README.md | 10 + husky_description/package.xml | 2 - husky_desktop/CHANGELOG.rst | 38 + husky_desktop/CMakeLists.txt | 4 + husky_desktop/package.xml | 21 + husky_gazebo/CHANGELOG.rst | 83 + husky_gazebo/CMakeLists.txt | 15 + husky_gazebo/env-hooks/50.husky_gazebo.sh | 1 + husky_gazebo/launch/husky_empty_world.launch | 49 + husky_gazebo/launch/husky_playpen.launch | 40 + husky_gazebo/launch/spawn_husky.launch | 97 + husky_gazebo/package.xml | 35 + .../accessories/kinect_camera.gazebo.xacro | 53 + husky_gazebo/urdf/description.gazebo.xacro | 45 + husky_gazebo/urdf/husky.gazebo.xacro | 95 + husky_gazebo/worlds/clearpath_playpen.world | 3469 +++++++++++++++++ husky_robot/CHANGELOG.rst | 54 + husky_robot/CMakeLists.txt | 4 + husky_robot/package.xml | 21 + husky_simulator/CHANGELOG.rst | 54 + husky_simulator/CMakeLists.txt | 4 + husky_simulator/package.xml | 21 + husky_ur5_moveit_config/.setup_assistant | 8 - husky_ur5_moveit_config/CHANGELOG.rst | 42 - husky_ur5_moveit_config/CMakeLists.txt | 11 - .../config/controllers.yaml | 6 - .../config/fake_controllers.yaml | 3 - husky_ur5_moveit_config/config/husky_ur5.srdf | 76 - .../config/joint_limits.yaml | 34 - .../config/kinematics.yaml | 5 - .../config/ompl_planning.yaml | 67 - husky_ur5_moveit_config/config/padding.yaml | 2 - .../config/sensors_rgbd.yaml | 14 - .../launch/default_warehouse_db.launch | 13 - husky_ur5_moveit_config/launch/demo.launch | 43 - .../fake_moveit_controller_manager.launch.xml | 9 - ...y_ur5_moveit_controller_manager.launch.xml | 8 - ...husky_ur5_moveit_sensor_manager.launch.xml | 6 - .../husky_ur5_planning_execution.launch | 13 - .../launch/joystick_control.launch | 17 - .../launch/move_group.launch | 69 - husky_ur5_moveit_config/launch/moveit.rviz | 689 ---- .../launch/moveit_rviz.launch | 16 - .../launch/ompl_planning_pipeline.launch.xml | 22 - .../launch/planning_context.launch | 25 - .../launch/planning_pipeline.launch.xml | 10 - .../launch/run_benchmark_ompl.launch | 22 - .../launch/sensor_manager.launch.xml | 14 - .../launch/setup_assistant.launch | 15 - .../launch/trajectory_execution.launch.xml | 18 - .../launch/warehouse.launch | 15 - .../launch/warehouse_settings.launch.xml | 15 - husky_ur5_moveit_config/package.xml | 36 - husky_viz/CHANGELOG.rst | 55 + husky_viz/CMakeLists.txt | 14 + husky_viz/launch/view_model.launch | 21 + husky_viz/launch/view_robot.launch | 4 + husky_viz/package.xml | 29 + husky_viz/rviz/model.rviz | 172 + husky_viz/rviz/robot.rviz | 385 ++ 113 files changed, 12541 insertions(+), 1389 deletions(-) create mode 100644 husky_base/CHANGELOG.rst create mode 100644 husky_base/CMakeLists.txt create mode 100644 husky_base/config/diagnostics.yaml create mode 100644 husky_base/include/husky_base/horizon_legacy/Exception.h create mode 100644 husky_base/include/husky_base/horizon_legacy/Logger.h create mode 100644 husky_base/include/husky_base/horizon_legacy/Message.h create mode 100644 husky_base/include/husky_base/horizon_legacy/Message_cmd.h create mode 100644 husky_base/include/husky_base/horizon_legacy/Message_data.h create mode 100644 husky_base/include/husky_base/horizon_legacy/Message_request.h create mode 100644 husky_base/include/husky_base/horizon_legacy/Number.h create mode 100644 husky_base/include/husky_base/horizon_legacy/Transport.h create mode 100644 husky_base/include/husky_base/horizon_legacy/clearpath.h create mode 100644 husky_base/include/husky_base/horizon_legacy/crc.h create mode 100644 husky_base/include/husky_base/horizon_legacy/serial.h create mode 100644 husky_base/include/husky_base/horizon_legacy_wrapper.h create mode 100644 husky_base/include/husky_base/husky_diagnostics.h create mode 100644 husky_base/include/husky_base/husky_hardware.h create mode 100644 husky_base/launch/base.launch create mode 100644 husky_base/package.xml create mode 100644 husky_base/src/horizon_legacy/Logger.cpp create mode 100644 husky_base/src/horizon_legacy/Message.cpp create mode 100644 husky_base/src/horizon_legacy/Message_cmd.cpp create mode 100644 husky_base/src/horizon_legacy/Message_data.cpp create mode 100644 husky_base/src/horizon_legacy/Message_request.cpp create mode 100644 husky_base/src/horizon_legacy/Number.cpp create mode 100644 husky_base/src/horizon_legacy/Transport.cpp create mode 100644 husky_base/src/horizon_legacy/crc.cpp create mode 100644 husky_base/src/horizon_legacy/linux_serial.cpp create mode 100644 husky_base/src/horizon_legacy_wrapper.cpp create mode 100644 husky_base/src/husky_base.cpp create mode 100644 husky_base/src/husky_diagnostics.cpp create mode 100644 husky_base/src/husky_hardware.cpp create mode 100644 husky_bringup/CHANGELOG.rst create mode 100644 husky_bringup/CMakeLists.txt create mode 100644 husky_bringup/config/mag_config_default.yaml create mode 100644 husky_bringup/debian/ros-indigo-husky-bringup.udev create mode 100644 husky_bringup/env-hooks/50.husky_find_mag_config.sh create mode 100644 husky_bringup/launch/lms1xx_config/lms1xx.launch create mode 100644 husky_bringup/launch/microstrain_config/microstrain.launch create mode 100644 husky_bringup/launch/navsat_config/navsat.launch create mode 100644 husky_bringup/launch/um6_config/um6.launch create mode 100644 husky_bringup/launch/um7_config/um7.launch create mode 100644 husky_bringup/launch/ur5_arm_config/ur5.launch create mode 100644 husky_bringup/package.xml create mode 100755 husky_bringup/scripts/calibrate_compass create mode 100755 husky_bringup/scripts/compute_calibration create mode 100755 husky_bringup/scripts/install create mode 100644 husky_bringup/udev/10-microstrain.rules create mode 100644 husky_bringup/udev/41-clearpath.rules create mode 100644 husky_bringup/udev/41-hokuyo.rules create mode 100644 husky_bringup/udev/52-ftdi.rules create mode 100644 husky_bringup/udev/README.md create mode 100644 husky_desktop/CHANGELOG.rst create mode 100644 husky_desktop/CMakeLists.txt create mode 100644 husky_desktop/package.xml create mode 100644 husky_gazebo/CHANGELOG.rst create mode 100644 husky_gazebo/CMakeLists.txt create mode 100644 husky_gazebo/env-hooks/50.husky_gazebo.sh create mode 100644 husky_gazebo/launch/husky_empty_world.launch create mode 100644 husky_gazebo/launch/husky_playpen.launch create mode 100644 husky_gazebo/launch/spawn_husky.launch create mode 100644 husky_gazebo/package.xml create mode 100644 husky_gazebo/urdf/accessories/kinect_camera.gazebo.xacro create mode 100644 husky_gazebo/urdf/description.gazebo.xacro create mode 100644 husky_gazebo/urdf/husky.gazebo.xacro create mode 100644 husky_gazebo/worlds/clearpath_playpen.world create mode 100644 husky_robot/CHANGELOG.rst create mode 100644 husky_robot/CMakeLists.txt create mode 100644 husky_robot/package.xml create mode 100644 husky_simulator/CHANGELOG.rst create mode 100644 husky_simulator/CMakeLists.txt create mode 100644 husky_simulator/package.xml delete mode 100644 husky_ur5_moveit_config/.setup_assistant delete mode 100644 husky_ur5_moveit_config/CHANGELOG.rst delete mode 100644 husky_ur5_moveit_config/CMakeLists.txt delete mode 100644 husky_ur5_moveit_config/config/controllers.yaml delete mode 100644 husky_ur5_moveit_config/config/fake_controllers.yaml delete mode 100644 husky_ur5_moveit_config/config/husky_ur5.srdf delete mode 100644 husky_ur5_moveit_config/config/joint_limits.yaml delete mode 100644 husky_ur5_moveit_config/config/kinematics.yaml delete mode 100644 husky_ur5_moveit_config/config/ompl_planning.yaml delete mode 100644 husky_ur5_moveit_config/config/padding.yaml delete mode 100644 husky_ur5_moveit_config/config/sensors_rgbd.yaml delete mode 100644 husky_ur5_moveit_config/launch/default_warehouse_db.launch delete mode 100644 husky_ur5_moveit_config/launch/demo.launch delete mode 100644 husky_ur5_moveit_config/launch/fake_moveit_controller_manager.launch.xml delete mode 100644 husky_ur5_moveit_config/launch/husky_ur5_moveit_controller_manager.launch.xml delete mode 100644 husky_ur5_moveit_config/launch/husky_ur5_moveit_sensor_manager.launch.xml delete mode 100644 husky_ur5_moveit_config/launch/husky_ur5_planning_execution.launch delete mode 100644 husky_ur5_moveit_config/launch/joystick_control.launch delete mode 100644 husky_ur5_moveit_config/launch/move_group.launch delete mode 100644 husky_ur5_moveit_config/launch/moveit.rviz delete mode 100644 husky_ur5_moveit_config/launch/moveit_rviz.launch delete mode 100644 husky_ur5_moveit_config/launch/ompl_planning_pipeline.launch.xml delete mode 100644 husky_ur5_moveit_config/launch/planning_context.launch delete mode 100644 husky_ur5_moveit_config/launch/planning_pipeline.launch.xml delete mode 100644 husky_ur5_moveit_config/launch/run_benchmark_ompl.launch delete mode 100644 husky_ur5_moveit_config/launch/sensor_manager.launch.xml delete mode 100644 husky_ur5_moveit_config/launch/setup_assistant.launch delete mode 100644 husky_ur5_moveit_config/launch/trajectory_execution.launch.xml delete mode 100644 husky_ur5_moveit_config/launch/warehouse.launch delete mode 100644 husky_ur5_moveit_config/launch/warehouse_settings.launch.xml delete mode 100644 husky_ur5_moveit_config/package.xml create mode 100644 husky_viz/CHANGELOG.rst create mode 100644 husky_viz/CMakeLists.txt create mode 100644 husky_viz/launch/view_model.launch create mode 100644 husky_viz/launch/view_robot.launch create mode 100644 husky_viz/package.xml create mode 100644 husky_viz/rviz/model.rviz create mode 100644 husky_viz/rviz/robot.rviz diff --git a/.gitignore b/.gitignore index f8bcd1173..0e0497600 100644 --- a/.gitignore +++ b/.gitignore @@ -1,47 +1,6 @@ -build/ -bin/ -lib/ -msg_gen/ -srv_gen/ -msg/*Action.msg -msg/*ActionFeedback.msg -msg/*ActionGoal.msg -msg/*ActionResult.msg -msg/*Feedback.msg -msg/*Goal.msg -msg/*Result.msg -msg/_*.py - -# Generated by dynamic reconfigure -*.cfgc -/cfg/cpp/ -/cfg/*.py - -# Ignore generated docs -*.dox -*.wikidoc - -# eclipse stuff -.project -.cproject - -# qcreator stuff -CMakeLists.txt.user - -srv/_*.py -*.pcd -*.pyc -qtcreator-* -*.user - -/planning/cfg -/planning/docs -/planning/src - *~ -# Emacs -.#* - # Catkin custom files CATKIN_IGNORE + +*.pyc diff --git a/README.md b/README.md index 7f171aae4..073613f7d 100644 --- a/README.md +++ b/README.md @@ -8,8 +8,35 @@ real robot operation. - husky_description : Robot description (URDF) - husky_msgs : Message definitions - husky_navigation : Navigation configurations and demos - - husky_ur5_moveit_config : MoveIt configuration and demos For Husky instructions and tutorials, please see [Robots/Husky](http://wiki.ros.org/Robots/Husky). To create a custom Husky description or simulation, please fork [husky_customization](https://github.com/husky/husky_customization). + +husky_desktop +============= + +Desktop ROS packages for the Clearpath Husky, which may pull in graphical dependencies. + + - husky_viz : Visualization (rviz) configuration and bringup + +For Husky instructions and tutorials, please see http://wiki.ros.org/Robots/Husky + +husky_robot +=========== + +Robot ROS packages for the Clearpath Husky, for operating robot hardware. + + - husky_bringup : Bringup launch files and scripts. + - husky_base : Hardware driver for communicating with the onboard MCU. + +For Husky instructions and tutorials, please see http://wiki.ros.org/Robots/Husky + +husky_simualtor +============== + +Simulator ROS packages for the Clearpath Husky. + + - husky_gazebo : Gazebo plugin definitions and extensions to the robot URDF. + +For Husky instructions and tutorials, please see http://wiki.ros.org/Robots/Husky diff --git a/husky_base/CHANGELOG.rst b/husky_base/CHANGELOG.rst new file mode 100644 index 000000000..96d656137 --- /dev/null +++ b/husky_base/CHANGELOG.rst @@ -0,0 +1,98 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package husky_base +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.6 (2016-10-03) +------------------ +* Adding support for the UM7 IMU. +* Contributors: Tony Baltovski + +0.2.5 (2015-12-31) +------------------ +* Fix absolute value to handle negative rollover readings effectively +* Another bitwise fix, now for x86. +* Formatting +* Fix length complement check. + There's a subtle difference in how ~ is implemented in aarch64 which + causes this check to fail. The new implementation should work on x86 + and ARM. +* Contributors: Mike Purvis, Paul Bovbel + +0.2.4 (2015-07-08) +------------------ + +0.2.3 (2015-04-08) +------------------ +* Integrate husky_customization workflow +* Contributors: Paul Bovbel + +0.2.2 (2015-03-23) +------------------ +* Fix package urls +* Contributors: Paul Bovbel + +0.2.1 (2015-03-23) +------------------ +* Add missing dependencies +* Contributors: Paul Bovbel + +0.2.0 (2015-03-23) +------------------ +* Add UR5_ENABLED envvar +* Contributors: Paul Bovbel + +0.1.5 (2015-02-19) +------------------ +* Fix duration cast +* Contributors: Paul Bovbel + +0.1.4 (2015-02-13) +------------------ +* Correct issues with ROS time discontinuities - now using monotonic time source +* Implement a sane retry policy for communication with MCU +* Contributors: Paul Bovbel + +0.1.3 (2015-01-30) +------------------ +* Update description and maintainers +* Contributors: Paul Bovbel + +0.1.2 (2015-01-20) +------------------ +* Fix library install location +* Contributors: Paul Bovbel + +0.1.1 (2015-01-13) +------------------ +* Add missing description dependency +* Contributors: Paul Bovbel + +0.1.0 (2015-01-12) +------------------ +* Fixed encoder overflow issue +* Ported to ros_control for Indigo release +* Contributors: Mike Purvis, Paul Bovbel, finostro + +0.0.5 (2013-10-04) +------------------ +* Mark the config directory to install. + +0.0.4 (2013-10-03) +------------------ +* Parameterize husky port in env variable. + +0.0.3 (2013-09-24) +------------------ +* Add launchfile check. +* removing imu processing by dead_reckoning.py +* removing dynamic reconfigure from dead_reckoning because it was only there for handling gyro correction +* adding diagnostic aggregator and its related config file under config/diag_agg.yaml + +0.0.2 (2013-09-11) +------------------ +* Fix diagnostic_msgs dependency. + +0.0.1 (2013-09-11) +------------------ +* New husky_base package for Hydro, which contains the nodes + formerly in husky_bringup. diff --git a/husky_base/CMakeLists.txt b/husky_base/CMakeLists.txt new file mode 100644 index 000000000..4115e43fd --- /dev/null +++ b/husky_base/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 2.8.3) +project(husky_base) + +find_package(catkin REQUIRED COMPONENTS controller_manager hardware_interface husky_msgs + diagnostic_updater roslaunch roslint roscpp sensor_msgs) +find_package(Boost REQUIRED COMPONENTS chrono) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES horizon_legacy + CATKIN_DEPENDS diagnostic_updater hardware_interface husky_msgs roscpp sensor_msgs + DEPENDS Boost +) + +include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) + +add_library(horizon_legacy + src/horizon_legacy/crc.cpp + src/horizon_legacy/Logger.cpp + src/horizon_legacy/Message.cpp + src/horizon_legacy/Message_data.cpp + src/horizon_legacy/Message_request.cpp + src/horizon_legacy/Message_cmd.cpp + src/horizon_legacy/Transport.cpp + src/horizon_legacy/Number.cpp + src/horizon_legacy/linux_serial.cpp +) + +add_executable(husky_node + src/husky_base.cpp + src/husky_hardware.cpp + src/husky_diagnostics.cpp + src/horizon_legacy_wrapper.cpp) +target_link_libraries(husky_node horizon_legacy ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_dependencies(husky_node husky_msgs_gencpp) + +roslint_cpp( + src/husky_base.cpp + src/husky_hardware.cpp + src/husky_diagnostics.cpp + include/husky_base/husky_diagnostics.h + include/husky_base/husky_hardware.h +) + +roslaunch_add_file_check(launch) + +install(TARGETS horizon_legacy husky_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/husky_base/config/diagnostics.yaml b/husky_base/config/diagnostics.yaml new file mode 100644 index 000000000..f6feada46 --- /dev/null +++ b/husky_base/config/diagnostics.yaml @@ -0,0 +1,7 @@ +analyzers: + husky: + type: diagnostic_aggregator/GenericAnalyzer + path: Husky A200 + find_and_remove_prefix: 'husky_base: ' + expected: ['husky_base: power_status','husky_base: system_status','husky_base: safety_status','husky_base: software_status'] + timeout: 5.0 diff --git a/husky_base/include/husky_base/horizon_legacy/Exception.h b/husky_base/include/husky_base/horizon_legacy/Exception.h new file mode 100644 index 000000000..4f7b62fe8 --- /dev/null +++ b/husky_base/include/husky_base/horizon_legacy/Exception.h @@ -0,0 +1,68 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Exception.h +* Desc: Provides the clearpath::Exception class, which is the parent class +* for all clearpath exceptions. +* Auth: Iain Peet +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CPR_EXCEPTION_H +#define CPR_EXCEPTION_H + +#include + +namespace clearpath +{ + + class Exception + { + public: + const char *message; + + protected: + Exception(const char *msg = "none") : message(msg) + { + } + }; + +}; // namespace clearpath + +#endif // CPR_EXCEPTION_H diff --git a/husky_base/include/husky_base/horizon_legacy/Logger.h b/husky_base/include/husky_base/horizon_legacy/Logger.h new file mode 100644 index 000000000..e882198fb --- /dev/null +++ b/husky_base/include/husky_base/horizon_legacy/Logger.h @@ -0,0 +1,112 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Logger.h +* Desc: Provides the Logger singleton which is used within the Clearpath API +* for log / trace message control +* Auth: Iain Peet +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CPR_LOGGER_H +#define CPR_LOGGER_H + +#include + +namespace clearpath +{ + + class Logger + { + private: + bool enabled; + int level; + + std::ostream *stream; + + std::ofstream *nullStream; //i.e /dev/null + + public: + enum logLevels + { + ERROR_LEV, + EXCEPTION, + WARNING, + INFO, + DETAIL + }; + static const char *levelNames[]; // strings indexed by enumeration. + + private: + Logger(); + + ~Logger(); + + void close(); + + public: + static Logger &instance(); + + std::ostream &entry(enum logLevels level, const char *file = 0, int line = -1); + + void setEnabled(bool enabled); + + void setLevel(enum logLevels newLevel); + + void setStream(std::ostream *stream); + + void hookFatalSignals(); + + friend void loggerTermHandler(int signal); + }; + + void loggerTermHandler(int signal); + +}; // namespace clearpath + +// convenience macros +#define CPR_LOG(level) (clearpath::Logger::instance().entry((level), __FILE__, __LINE__ )) +#define CPR_ERR() CPR_LOG(clearpath::Logger::ERROR) +#define CPR_EXCEPT() (clearpath::Logger::instance().entry(clearpath::Logger::EXCEPTION)) +#define CPR_WARN() CPR_LOG(clearpath::Logger::WARNING) +#define CPR_INFO() CPR_LOG(clearpath::Logger::INFO) +#define CPR_DTL() CPR_LOG(clearpath::Logger::DETAIL) + +#endif //CPR_LOGGER_H + diff --git a/husky_base/include/husky_base/horizon_legacy/Message.h b/husky_base/include/husky_base/horizon_legacy/Message.h new file mode 100644 index 000000000..ad5909c9b --- /dev/null +++ b/husky_base/include/husky_base/horizon_legacy/Message.h @@ -0,0 +1,320 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: Message.h +* Desc: Definition for the Message class. This class represents a +* single message which is sent or received from a platform +* Auth: R. Gariepy, Iain Peet +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CLEARPATH_MESSAGE_H +#define CLEARPATH_MESSAGE_H + +#include +#include +#include + +#include "husky_base/horizon_legacy/Exception.h" + + +namespace clearpath +{ + + class MessageException : public Exception + { + public: + enum errors + { + ERROR_BASE = 0, + INVALID_LENGTH + }; + + public: + enum errors type; + + MessageException(const char *msg, enum errors ex_type = ERROR_BASE); + }; + + class Message + { + public: + static const size_t MAX_MSG_LENGTH = 256; + + + protected: + static const size_t CRC_LENGTH = 2; + static const uint16_t CRC_INIT_VAL = 0xFFFF; + + static const size_t HEADER_LENGTH = 12; + + // Offsets of fields within data + enum dataOffsets + { + SOH_OFST = 0, + LENGTH_OFST, + LENGTH_COMP_OFST, + VERSION_OFST, + TIMESTAMP_OFST, + FLAGS_OFST = 8, + TYPE_OFST, + STX_OFST = 11, + PAYLOAD_OFST + }; + + uint8_t data[MAX_MSG_LENGTH]; + // Total length (incl. full header & checksum) + size_t total_len; + + // Whether this Message has ever been sent by the Transport() + // (Updated by Transport::send()) + bool is_sent; + + friend class Transport; // Allow Transport to read data and total_len directly + + public: + static const size_t MIN_MSG_LENGTH = HEADER_LENGTH + CRC_LENGTH; + static const uint8_t SOH = 0xAA; + static const uint8_t STX = 0x55; + + protected: + size_t crcOffset() + { + return total_len - CRC_LENGTH; + }; + + void setLength(uint8_t len); + + void setVersion(uint8_t version); + + void setTimestamp(uint32_t timestamp); + + void setFlags(uint8_t flags); + + void setType(uint16_t type); + + uint8_t *getPayloadPointer(size_t offset = 0); + + void setPayload(void *buf, size_t buf_size); + + void setPayloadLength(uint8_t len); + + void makeValid(); + + public: + Message(); + + Message(void *input, size_t msg_len); + + Message(const Message &other); + + Message(uint16_t type, uint8_t *payload, size_t payload_len, + uint32_t timestamp = 0, uint8_t flags = 0, uint8_t version = 0); + + virtual ~Message(); + + void send(); + + uint8_t getLength(); // as reported by packet length field. + uint8_t getLengthComp(); + + uint8_t getVersion(); + + uint32_t getTimestamp(); + + uint8_t getFlags(); + + uint16_t getType(); + + uint16_t getChecksum(); + + size_t getPayloadLength() + { + return total_len - HEADER_LENGTH - CRC_LENGTH; + } + + size_t getPayload(void *buf, size_t max_size); + + size_t getTotalLength() + { + return total_len; + } + + size_t toBytes(void *buf, size_t buf_size); + + bool isValid(char *whyNot = NULL, size_t strLen = 0); + + bool isCommand() + { + return getType() < 0x4000; + } + + bool isRequest() + { + return (getType() >= 0x4000) && (getType() < 0x8000); + } + + bool isData() + { + return (getType() >= 0x8000) && (getType() < 0xC000); + } + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + + void printRaw(std::ostream &stream = std::cout); + + + static Message *factory(void *input, size_t msg_len); + + static Message *popNext(); + + static Message *waitNext(double timeout = 0.0); + + }; // class Message + + enum MessageTypes + { + /* + * Set commands + */ + SET_PLATFORM_NAME = 0x0002, + SET_PLATFORM_TIME = 0x0005, + SET_SAFETY_SYSTEM = 0x0010, + SET_DIFF_WHEEL_SPEEDS = 0x0200, + SET_DIFF_CTRL_CONSTS = 0x0201, + SET_DIFF_WHEEL_SETPTS = 0x0202, + SET_ACKERMANN_SETPT = 0x0203, + SET_VELOCITY_SETPT = 0x0204, + SET_TURN_SETPT = 0x0205, + SET_MAX_SPEED = 0x0210, + SET_MAX_ACCEL = 0x0211, + SET_GEAR_SETPOINT = 0x0212, + SET_GPADC_OUTPUT = 0x0300, + SET_GPIO_DIRECTION = 0x0301, + SET_GPIO_OUTPUT = 0x0302, + SET_PTZ_POSITION = 0x0400, + + /* + * Command commands + */ + CMD_PROCESSOR_RESET = 0x2000, + CMD_RESTORE_SETTINGS = 0x2001, + CMD_STORE_SETTINGS = 0x2002, + + /* + * Request commands + */ + REQUEST_ECHO = 0x4000, + REQUEST_PLATFORM_INFO = 0x4001, + REQUEST_PLATFORM_NAME = 0x4002, + REQUEST_FIRMWARE_INFO = 0x4003, + REQUEST_SYSTEM_STATUS = 0x4004, + REQUEST_POWER_SYSTEM = 0x4005, + REQUEST_SAFETY_SYSTEM = 0x4010, + REQUEST_DIFF_WHEEL_SPEEDS = 0x4200, + REQUEST_DIFF_CTRL_CONSTS = 0x4201, + REQUEST_DIFF_WHEEL_SETPTS = 0x4202, + REQUEST_ACKERMANN_SETPTS = 0x4203, + REQUEST_VELOCITY_SETPT = 0x4204, + REQUEST_TURN_SETPT = 0x4205, + REQUEST_MAX_SPEED = 0x4210, + REQUEST_MAX_ACCEL = 0x4211, + REQUEST_GEAR_SETPT = 0x4212, + REQUEST_GPADC_OUTPUT = 0x4300, + REQUEST_GPIO_STATUS = 0x4301, + REQUEST_GPADC_INPUT = 0x4303, + REQUEST_PTZ_POSITION = 0x4400, + REQUEST_DISTANCE_DATA = 0x4500, + REQUEST_DISTANCE_TIMING = 0x4501, + REQUEST_ORIENT = 0x4600, + REQUEST_ROT_RATE = 0x4601, + REQUEST_ACCEL = 0x4602, + REQUEST_6AXIS = 0x4603, + REQUEST_6AXIS_ORIENT = 0x4604, + REQUEST_ENCODER = 0x4800, + REQUEST_ENCODER_RAW = 0x4801, + + /* + * Data commands + */ + DATA_ECHO = 0x8000, + DATA_PLATFORM_INFO = 0x8001, + DATA_PLATFORM_NAME = 0x8002, + DATA_FIRMWARE_INFO = 0x8003, + DATA_SYSTEM_STATUS = 0x8004, + DATA_POWER_SYSTEM = 0x8005, + DATA_PROC_STATUS = 0x8006, + DATA_SAFETY_SYSTEM = 0x8010, + DATA_DIFF_WHEEL_SPEEDS = 0x8200, + DATA_DIFF_CTRL_CONSTS = 0x8201, + DATA_DIFF_WHEEL_SETPTS = 0x8202, + DATA_ACKERMANN_SETPTS = 0x8203, + DATA_VELOCITY_SETPT = 0x8204, + DATA_TURN_SETPT = 0x8205, + DATA_MAX_SPEED = 0x8210, + DATA_MAX_ACCEL = 0x8211, + DATA_GEAR_SETPT = 0x8212, + DATA_GPADC_OUTPUT = 0x8300, + DATA_GPIO_STATUS = 0x8301, + DATA_GPADC_INPUT = 0x8303, + DATA_PTZ_POSITION = 0x8400, + DATA_DISTANCE_DATA = 0x8500, + DATA_DISTANCE_TIMING = 0x8501, + DATA_ORIENT = 0x8600, + DATA_ROT_RATE = 0x8601, + DATA_ACCEL = 0x8602, + DATA_6AXIS = 0x8603, + DATA_6AXIS_ORIENT = 0x8604, + DATA_MAGNETOMETER = 0x8606, + DATA_ENCODER = 0x8800, + DATA_ENCODER_RAW = 0x8801, + DATA_CURRENT_RAW = 0xA110, + DATA_VOLTAGE_RAW = 0xA111, + DATA_TEMPERATURE_RAW = 0xA112, + DATA_ORIENT_RAW = 0xA113, + DATA_GYRO_RAW = 0xA114, + DATA_ACCEL_RAW = 0xA115, + DATA_MAGNETOMETER_RAW = 0xA116 + }; // enum MessageTypes + +}; // namespace clearpath + +std::ostream &operator<<(std::ostream &stream, clearpath::Message &msg); + +#endif // CLEARPATH_MESSAGE_H diff --git a/husky_base/include/husky_base/horizon_legacy/Message_cmd.h b/husky_base/include/husky_base/horizon_legacy/Message_cmd.h new file mode 100644 index 000000000..f9926dc2f --- /dev/null +++ b/husky_base/include/husky_base/horizon_legacy/Message_cmd.h @@ -0,0 +1,298 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Message_cmd.h +* Desc: Provides Set Message subclasses. +* Auth: Iain Peet +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CLEARPATH_MESSAGE_CMD_H +#define CLEARPATH_MESSAGE_CMD_H + +#include "husky_base/horizon_legacy/Message.h" + +namespace clearpath +{ + + class CmdMessage : public Message + { + private: + static long total_destroyed; + static long total_sent; + + public: + CmdMessage() : Message() + { + } + + CmdMessage(const CmdMessage &other) : Message(other) + { + } + + virtual ~CmdMessage(); + }; + + class CmdProcessorReset : public CmdMessage + { + public: + CmdProcessorReset(); + + CmdProcessorReset(const CmdProcessorReset &other); + }; + + class CmdRestoreSettings : public CmdMessage + { + public: + enum restoreFlags + { + USER_SETTINGS = 0x1, + FACTORY_SETTINGS = 0x2 + }; + public: + CmdRestoreSettings(enum restoreFlags flags); + + CmdRestoreSettings(const CmdRestoreSettings &other); + }; + + class CmdStoreSettings : public CmdMessage + { + public: + CmdStoreSettings(); + + CmdStoreSettings(const CmdStoreSettings &other); + }; + + class SetAckermannOutput : public CmdMessage + { + public: + enum payloadOffsets + { + STEERING = 0, + THROTTLE = 2, + BRAKE = 4, + PAYLOAD_LEN = 6 + }; + + public: + SetAckermannOutput(double steering, double throt, double brake); + + SetAckermannOutput(const SetAckermannOutput &other); + }; + + class SetDifferentialControl : public CmdMessage + { + public: + enum payloadOffsets + { + LEFT_P = 0, + LEFT_I = 2, + LEFT_D = 4, + LEFT_FEEDFWD = 6, + LEFT_STIC = 8, + LEFT_INT_LIM = 10, + RIGHT_P = 12, + RIGHT_I = 14, + RIGHT_D = 16, + RIGHT_FEEDFWD = 18, + RIGHT_STIC = 20, + RIGHT_INT_LIM = 22, + PAYLOAD_LEN = 24 + }; + + public: + SetDifferentialControl(double p, + double i, + double d, + double feedfwd, + double stic, + double int_lim); + + SetDifferentialControl(double left_p, + double left_i, + double left_d, + double left_feedfwd, + double left_stic, + double left_int_lim, + double right_p, + double right_i, + double right_d, + double right_feedfwd, + double right_stic, + double right_int_lim); + + SetDifferentialControl(const SetDifferentialControl &other); + }; + + class SetDifferentialOutput : public CmdMessage + { + public: + enum payloadOffsets + { + LEFT = 0, + RIGHT = 2, + PAYLOAD_LEN = 4 + }; + + public: + SetDifferentialOutput(double left, double right); + + SetDifferentialOutput(const SetDifferentialOutput &other); + }; + + class SetDifferentialSpeed : public CmdMessage + { + public: + enum payloadOffsets + { + LEFT_SPEED = 0, + RIGHT_SPEED = 2, + LEFT_ACCEL = 4, + RIGHT_ACCEL = 6, + PAYLOAD_LEN = 8 + }; + + public: + SetDifferentialSpeed(double left_spd, double right_speed, double left_accel, double right_accel); + + SetDifferentialSpeed(const SetDifferentialSpeed &other); + }; + + class SetGear : public CmdMessage + { + public: + SetGear(uint8_t gear); + + SetGear(const SetGear &other); + }; + + class SetMaxAccel : public CmdMessage + { + public: + enum payloadOffsets + { + MAX_FWD = 0, + MAX_REV = 2, + PAYLOAD_LEN = 4 + }; + + public: + SetMaxAccel(double max_fwd, double max_rev); + + SetMaxAccel(const SetMaxAccel &other); + }; + + class SetMaxSpeed : public CmdMessage + { + public: + enum payloadOffsets + { + MAX_FWD = 0, + MAX_REV = 2, + PAYLOAD_LEN = 4 + }; + + public: + SetMaxSpeed(double max_fwd, double max_rev); + + SetMaxSpeed(const SetMaxSpeed &other); + }; + + class SetPlatformName : public CmdMessage + { + public: + SetPlatformName(const char *name); + + SetPlatformName(const SetPlatformName &other); + }; + + class SetPlatformTime : public CmdMessage + { + public: + SetPlatformTime(uint32_t time); + + SetPlatformTime(const SetPlatformTime &other); + }; + + class SetSafetySystem : public CmdMessage + { + public: + SetSafetySystem(uint16_t flags); + + SetSafetySystem(const SetSafetySystem &other); + }; + + class SetTurn : public CmdMessage + { + public: + enum payloadOffsets + { + TRANSLATIONAL = 0, + TURN_RADIUS = 2, + TRANS_ACCEL = 4, + PAYLOAD_LEN = 6 + }; + + public: + SetTurn(double trans, double rad, double accel); + + SetTurn(const SetTurn &other); + }; + + class SetVelocity : public CmdMessage + { + public: + enum payloadOffsets + { + TRANSLATIONAL = 0, + ROTATIONAL = 2, + TRANS_ACCEL = 4, + PAYLOAD_LEN = 6 + }; + + public: + SetVelocity(double trans, double rot, double accel); + + SetVelocity(const SetVelocity &other); + }; + +}; // namespace clearpath + +#endif // CLEARPATH_MESSAGE_CMD_H + diff --git a/husky_base/include/husky_base/horizon_legacy/Message_data.h b/husky_base/include/husky_base/horizon_legacy/Message_data.h new file mode 100644 index 000000000..b96a479f1 --- /dev/null +++ b/husky_base/include/husky_base/horizon_legacy/Message_data.h @@ -0,0 +1,1090 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: HorizonMessage_data.h +* Desc: Provides Message subclasses corresponding to 'data' type messages +* Auth: Iain Peet, Mike Purvis +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + + +#ifndef CLEARPATH_MESSAGE_DATA_H +#define CLEARPATH_MESSAGE_DATA_H + +#include +#include +#include +#include "husky_base/horizon_legacy/Message.h" +#include "husky_base/horizon_legacy/Message_request.h" + +namespace clearpath +{ + + class DataAckermannOutput : public Message + { + public: + enum payloadOffsets + { + STEERING = 0, + THROTTLE = 2, + BRAKE = 4, + PAYLOAD_LEN = 6 + }; + public: + DataAckermannOutput(void *input, size_t msg_len); + + DataAckermannOutput(const DataAckermannOutput &other); + + static DataAckermannOutput *popNext(); + + static DataAckermannOutput *waitNext(double timeout = 0); + + static DataAckermannOutput *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq = 0); + + static enum MessageTypes getTypeID(); + + double getSteering(); + + double getThrottle(); + + double getBrake(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataDifferentialControl : public Message + { + public: + enum payloadOffsets + { + LEFT_P = 0, + LEFT_I = 2, + LEFT_D = 4, + LEFT_FEEDFWD = 6, + LEFT_STIC = 8, + LEFT_INT_LIM = 10, + RIGHT_P = 12, + RIGHT_I = 14, + RIGHT_D = 16, + RIGHT_FEEDFWD = 18, + RIGHT_STIC = 20, + RIGHT_INT_LIM = 22, + PAYLOAD_LEN = 24 + }; + + public: + DataDifferentialControl(void *input, size_t msg_len); + + DataDifferentialControl(const DataDifferentialControl &other); + + static DataDifferentialControl *popNext(); + + static DataDifferentialControl *waitNext(double timeout = 0); + + static DataDifferentialControl *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq = 0); + + static enum MessageTypes getTypeID(); + + double getLeftP(); + + double getLeftI(); + + double getLeftD(); + + double getLeftFeedForward(); + + double getLeftStiction(); + + double getLeftIntegralLimit(); + + double getRightP(); + + double getRightI(); + + double getRightD(); + + double getRightFeedForward(); + + double getRightStiction(); + + double getRightIntegralLimit(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataDifferentialOutput : public Message + { + public: + enum payloadOffsets + { + LEFT = 0, + RIGHT = 2, + PAYLOAD_LEN = 4 + }; + + public: + DataDifferentialOutput(void *input, size_t msg_len); + + DataDifferentialOutput(const DataDifferentialOutput &other); + + static DataDifferentialOutput *popNext(); + + static DataDifferentialOutput *waitNext(double timeout = 0); + + static DataDifferentialOutput *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getLeft(); + + double getRight(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataDifferentialSpeed : public Message + { + public: + enum payloadOffsets + { + LEFT_SPEED = 0, + RIGHT_SPEED = 2, + LEFT_ACCEL = 4, + RIGHT_ACCEL = 6, + PAYLOAD_LEN = 8 + }; + + public: + DataDifferentialSpeed(void *input, size_t msg_len); + + DataDifferentialSpeed(const DataDifferentialSpeed &other); + + static DataDifferentialSpeed *popNext(); + + static DataDifferentialSpeed *waitNext(double timeout = 0); + + static DataDifferentialSpeed *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getLeftSpeed(); + + double getLeftAccel(); + + double getRightSpeed(); + + double getRightAccel(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataEcho : public Message + { + public: + DataEcho(void *input, size_t msg_len); + + DataEcho(const DataEcho &other); + + static DataEcho *popNext(); + + static DataEcho *waitNext(double timeout = 0); + + static DataEcho *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataEncoders : public Message + { + private: + size_t travels_offset; + size_t speeds_offset; + + public: + DataEncoders(void *input, size_t msg_len); + + DataEncoders(const DataEncoders &other); + + static DataEncoders *popNext(); + + static DataEncoders *waitNext(double timeout = 0); + + static DataEncoders *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getCount(); + + double getTravel(uint8_t index); + + double getSpeed(uint8_t index); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataEncodersRaw : public Message + { + public: + DataEncodersRaw(void *input, size_t pkt_len); + + DataEncodersRaw(const DataEncodersRaw &other); + + static DataEncodersRaw *popNext(); + + static DataEncodersRaw *waitNext(double timeout = 0); + + static DataEncodersRaw *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getCount(); + + int32_t getTicks(uint8_t inx); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataFirmwareInfo : public Message + { + public: + enum payloadOffsets + { + MAJOR_FIRM_VER = 0, + MINOR_FIRM_VER, + MAJOR_PROTO_VER, + MINOR_PROTO_VER, + WRITE_TIME, + PAYLOAD_LEN = 8 + }; + + class WriteTime + { + public: + uint32_t rawTime; + public: + WriteTime(uint32_t time) : rawTime(time) + { + } + + uint8_t minute() + { + return (rawTime) & 0x3f; + } + + uint8_t hour() + { + return (rawTime >> 6) & 0x1f; + } + + uint8_t day() + { + return (rawTime >> 11) & 0x3f; + } + + uint8_t month() + { + return (rawTime >> 17) & 0x0f; + } + + uint8_t year() + { + return (rawTime >> 21) & 0x7f; + } + }; + + public: + DataFirmwareInfo(void *input, size_t msg_len); + + DataFirmwareInfo(const DataFirmwareInfo &other); + + static DataFirmwareInfo *popNext(); + + static DataFirmwareInfo *waitNext(double timeout = 0); + + static DataFirmwareInfo *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getMajorFirmwareVersion(); + + uint8_t getMinorFirmwareVersion(); + + uint8_t getMajorProtocolVersion(); + + uint8_t getMinorProtocolVersion(); + + WriteTime getWriteTime(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataGear : public Message + { + public: + DataGear(void *input, size_t msg_len); + + DataGear(const DataGear &other); + + static DataGear *popNext(); + + static DataGear *waitNext(double timeout = 0); + + static DataGear *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getGear(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataMaxAcceleration : public Message + { + public: + enum payloadOffsets + { + FORWARD_MAX = 0, + REVERSE_MAX = 2, + PAYLOAD_LEN = 4 + }; + + public: + DataMaxAcceleration(void *input, size_t msg_len); + + DataMaxAcceleration(const DataMaxAcceleration &other); + + static DataMaxAcceleration *popNext(); + + static DataMaxAcceleration *waitNext(double timeout = 0); + + static DataMaxAcceleration *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getForwardMax(); + + double getReverseMax(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataMaxSpeed : public Message + { + public: + enum payloadOffsets + { + FORWARD_MAX = 0, + REVERSE_MAX = 2, + PAYLOAD_LEN = 4 + }; + + public: + DataMaxSpeed(void *input, size_t msg_len); + + DataMaxSpeed(const DataMaxSpeed &other); + + static DataMaxSpeed *popNext(); + + static DataMaxSpeed *waitNext(double timeout = 0); + + static DataMaxSpeed *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getForwardMax(); + + double getReverseMax(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPlatformAcceleration : public Message + { + public: + enum payloadOffsets + { + X = 0, + Y = 2, + Z = 4, + PAYLOAD_LEN = 6 + }; + public: + DataPlatformAcceleration(void *input, size_t msg_len); + + DataPlatformAcceleration(const DataPlatformAcceleration &other); + + static DataPlatformAcceleration *popNext(); + + static DataPlatformAcceleration *waitNext(double timeout = 0); + + static DataPlatformAcceleration *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq = 0); + + static enum MessageTypes getTypeID(); + + double getX(); + + double getY(); + + double getZ(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPlatformInfo : public Message + { + private: + uint8_t strlenModel(); + + public: + DataPlatformInfo(void *input, size_t msg_len); + + DataPlatformInfo(const DataPlatformInfo &other); + + static DataPlatformInfo *popNext(); + + static DataPlatformInfo *waitNext(double timeout = 0); + + static DataPlatformInfo *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + std::string getModel(); + + uint8_t getRevision(); + + uint32_t getSerial(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPlatformName : public Message + { + public: + DataPlatformName(void *input, size_t msg_len); + + DataPlatformName(const DataPlatformName &other); + + static DataPlatformName *popNext(); + + static DataPlatformName *waitNext(double timeout = 0); + + static DataPlatformName *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + std::string getName(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPlatformMagnetometer : public Message + { + public: + enum payloadOffsets + { + X = 0, + Y = 2, + Z = 4, + PAYLOAD_LEN = 6 + }; + public: + DataPlatformMagnetometer(void *input, size_t msg_len); + + DataPlatformMagnetometer(const DataPlatformMagnetometer &other); + + static DataPlatformMagnetometer *popNext(); + + static DataPlatformMagnetometer *waitNext(double timeout = 0); + + static DataPlatformMagnetometer *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getX(); + + double getY(); + + double getZ(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPlatformOrientation : public Message + { + public: + enum payloadOffsets + { + ROLL = 0, + PITCH = 2, + YAW = 4, + PAYLOAD_LEN = 6 + }; + public: + DataPlatformOrientation(void *input, size_t msg_len); + + DataPlatformOrientation(const DataPlatformOrientation &other); + + static DataPlatformOrientation *popNext(); + + static DataPlatformOrientation *waitNext(double timeout = 0); + + static DataPlatformOrientation *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getRoll(); + + double getYaw(); + + double getPitch(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPlatformRotation : public Message + { + public: + enum payloadOffsets + { + ROLL_RATE = 0, + PITCH_RATE = 2, + YAW_RATE = 4, + PAYLOAD_LEN = 6 + }; + + public: + DataPlatformRotation(void *input, size_t msg_len); + + DataPlatformRotation(const DataPlatformRotation &other); + + static DataPlatformRotation *popNext(); + + static DataPlatformRotation *waitNext(double timeout = 0); + + static DataPlatformRotation *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getRollRate(); + + double getPitchRate(); + + double getYawRate(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataPowerSystem : public Message + { + public: + class BatteryDescription + { + public: + enum Types + { + EXTERNAL = 0x0, + LEAD_ACID = 0x1, + NIMH = 0x2, + GASOLINE = 0x8 + }; + uint8_t rawDesc; + public: + BatteryDescription(uint8_t desc) : rawDesc(desc) + { + } + + bool isPresent() + { + return rawDesc & 0x80; + } + + bool isInUse() + { + return rawDesc & 0x40; + } + + enum Types getType() + { + return (enum Types) (rawDesc & 0x0f); + } + }; + + public: + DataPowerSystem(void *input, size_t msg_len); + + DataPowerSystem(const DataPowerSystem &other); + + static DataPowerSystem *popNext(); + + static DataPowerSystem *waitNext(double timeout = 0); + + static DataPowerSystem *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getBatteryCount(); + + double getChargeEstimate(uint8_t battery); + + int16_t getCapacityEstimate(uint8_t battery); + + BatteryDescription getDescription(uint8_t battery); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataProcessorStatus : public Message + { + public: + DataProcessorStatus(void *input, size_t msg_len); + + DataProcessorStatus(const DataProcessorStatus &other); + + static DataProcessorStatus *popNext(); + + static DataProcessorStatus *waitNext(double timeout = 0); + + static DataProcessorStatus *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getProcessCount(); + + int16_t getErrorCount(int process); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRangefinders : public Message + { + public: + DataRangefinders(void *input, size_t msg_len); + + DataRangefinders(const DataRangefinders &other); + + static DataRangefinders *popNext(); + + static DataRangefinders *waitNext(double timeout = 0); + + static DataRangefinders *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getRangefinderCount(); + + int16_t getDistance(int rangefinder); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRangefinderTimings : public Message + { + public: + DataRangefinderTimings(void *input, size_t msg_len); + + DataRangefinderTimings(const DataRangefinderTimings &other); + + static DataRangefinderTimings *popNext(); + + static DataRangefinderTimings *waitNext(double timeout = 0); + + static DataRangefinderTimings *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getRangefinderCount(); + + int16_t getDistance(int rangefinder); + + uint32_t getAcquisitionTime(int rangefinder); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawAcceleration : public Message + { + public: + enum payloadOffsets + { + X = 0, + Y = 2, + Z = 4, + PAYLOAD_LEN = 6 + }; + public: + DataRawAcceleration(void *input, size_t msg_len); + + DataRawAcceleration(const DataRawAcceleration &other); + + static DataRawAcceleration *popNext(); + + static DataRawAcceleration *waitNext(double timeout = 0); + + static DataRawAcceleration *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint16_t getX(); + + uint16_t getY(); + + uint16_t getZ(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawCurrent : public Message + { + public: + DataRawCurrent(void *input, size_t msg_len); + + DataRawCurrent(const DataRawCurrent &other); + + static DataRawCurrent *popNext(); + + static DataRawCurrent *waitNext(double timeout = 0); + + static DataRawCurrent *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getCurrentCount(); + + uint16_t getCurrent(int current); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawGyro : public Message + { + public: + enum payloadOffsets + { + ROLL = 0, + PITCH = 2, + YAW = 4, + PAYLOAD_LEN = 6 + }; + public: + DataRawGyro(void *input, size_t msg_len); + + DataRawGyro(const DataRawGyro &other); + + static DataRawGyro *popNext(); + + static DataRawGyro *waitNext(double timeout = 0); + + static DataRawGyro *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint16_t getRoll(); + + uint16_t getPitch(); + + uint16_t getYaw(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawMagnetometer : public Message + { + public: + enum payloadOffsets + { + X = 0, + Y = 2, + Z = 4, + PAYLOAD_LEN = 6 + }; + public: + DataRawMagnetometer(void *input, size_t msg_len); + + DataRawMagnetometer(const DataRawMagnetometer &other); + + static DataRawMagnetometer *popNext(); + + static DataRawMagnetometer *waitNext(double timeout = 0); + + static DataRawMagnetometer *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint16_t getX(); + + uint16_t getY(); + + uint16_t getZ(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawOrientation : public Message + { + public: + enum payloadOffsets + { + ROLL = 0, + PITCH = 2, + YAW = 4, + PAYLOAD_LEN = 6 + }; + public: + DataRawOrientation(void *input, size_t msg_len); + + DataRawOrientation(const DataRawOrientation &other); + + static DataRawOrientation *popNext(); + + static DataRawOrientation *waitNext(double timeout = 0); + + static DataRawOrientation *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint16_t getRoll(); + + uint16_t getPitch(); + + uint16_t getYaw(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawTemperature : public Message + { + public: + DataRawTemperature(void *input, size_t msg_len); + + DataRawTemperature(const DataRawTemperature &other); + + static DataRawTemperature *popNext(); + + static DataRawTemperature *waitNext(double timeout = 0); + + static DataRawTemperature *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getTemperatureCount(); + + uint16_t getTemperature(int temperature); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataRawVoltage : public Message + { + public: + DataRawVoltage(void *input, size_t msg_len); + + DataRawVoltage(const DataRawVoltage &other); + + static DataRawVoltage *popNext(); + + static DataRawVoltage *waitNext(double timeout = 0); + + static DataRawVoltage *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint8_t getVoltageCount(); + + uint16_t getVoltage(int temperature); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataSafetySystemStatus : public Message + { + public: + DataSafetySystemStatus(void *input, size_t msg_len); + + DataSafetySystemStatus(const DataSafetySystemStatus &other); + + static DataSafetySystemStatus *popNext(); + + static DataSafetySystemStatus *waitNext(double timeout = 0); + + static DataSafetySystemStatus *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint16_t getFlags(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataSystemStatus : public Message + { + private: + uint8_t voltages_offset; + uint8_t currents_offset; + uint8_t temperatures_offset; + + public: + DataSystemStatus(void *input, size_t msg_len); + + DataSystemStatus(const DataSystemStatus &other); + + static DataSystemStatus *popNext(); + + static DataSystemStatus *waitNext(double timeout = 0); + + static DataSystemStatus *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + uint32_t getUptime(); + + uint8_t getVoltagesCount(); + + double getVoltage(uint8_t index); + + uint8_t getCurrentsCount(); + + double getCurrent(uint8_t index); + + uint8_t getTemperaturesCount(); + + double getTemperature(uint8_t index); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + + class DataVelocity : public Message + { + public: + enum payloadOffsets + { + TRANS_VEL = 0, + ROTATIONAL = 2, + TRANS_ACCEL = 4, + PAYLOAD_LEN = 6 + }; + public: + DataVelocity(void *input, size_t msg_len); + + DataVelocity(const DataVelocity &other); + + static DataVelocity *popNext(); + + static DataVelocity *waitNext(double timeout = 0); + + static DataVelocity *getUpdate(double timeout = 0); + + static void subscribe(uint16_t freq); + + static enum MessageTypes getTypeID(); + + double getTranslational(); + + double getRotational(); + + double getTransAccel(); + + virtual std::ostream &printMessage(std::ostream &stream = std::cout); + }; + +}; // namespace clearpath + +#endif // CLEARPATH_MESSAGE_DATA_H + diff --git a/husky_base/include/husky_base/horizon_legacy/Message_request.h b/husky_base/include/husky_base/horizon_legacy/Message_request.h new file mode 100644 index 000000000..b736ccd0f --- /dev/null +++ b/husky_base/include/husky_base/horizon_legacy/Message_request.h @@ -0,0 +1,66 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Message_request.h +* Desc: Provides the Request class, which is used to request data messages +* at a particular frequency. +* Auth: Iain Peet +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CLEARPATH_MESSAGE_REQUEST_H +#define CLEARPATH_MESSAGE_REQUEST_H + +#include "husky_base/horizon_legacy/Message.h" + +namespace clearpath +{ + + class Request : public Message + { + public: + Request(uint16_t type, uint16_t freq = 0); + + Request(const Request &other); + }; + +}; // namespace clearpath + +#endif // CLEARPATH_MESSAGE_REQUEST_H + diff --git a/husky_base/include/husky_base/horizon_legacy/Number.h b/husky_base/include/husky_base/horizon_legacy/Number.h new file mode 100644 index 000000000..16ecc9bfd --- /dev/null +++ b/husky_base/include/husky_base/horizon_legacy/Number.h @@ -0,0 +1,85 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Number.h +* Desc: Provides a family of functions similar in form to stdlib atoi and +* friends, for conversion between numeric primitives and small-endian +* byte arrays. +* Auth: Iain Peet +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef NUMBER_H_ +#define NUMBER_H_ + +#include +#include +#include + +namespace clearpath +{ + +/* Little-endian byte array to number conversion routines. */ + void utob(void *dest, size_t dest_len, uint64_t src); + + void utob(void *dest, size_t dest_len, uint32_t src); + + void utob(void *dest, size_t dest_len, uint16_t src); + + void itob(void *dest, size_t dest_len, int64_t src); + + void itob(void *dest, size_t dest_len, int32_t src); + + void itob(void *dest, size_t dest_len, int16_t src); + +/* void toBytes(void* dest, size_t dest_len, float src, float scale); */ + void ftob(void *dest, size_t dest_len, double src, double scale); + +/* Number to little-endian byte array conversion routines + * Need to provide all, since size of the int param matters. */ + uint64_t btou(void *src, size_t src_len); + + int64_t btoi(void *src, size_t src_len); + + double btof(void *src, size_t src_len, double scale); + +}; // namespace clearpath + +#endif // NUMBER_H_ + diff --git a/husky_base/include/husky_base/horizon_legacy/Transport.h b/husky_base/include/husky_base/horizon_legacy/Transport.h new file mode 100644 index 000000000..80f58e7d5 --- /dev/null +++ b/husky_base/include/husky_base/horizon_legacy/Transport.h @@ -0,0 +1,180 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: Transport.h +* Desc: Definition for Horizon transport class. Implements the details of +* the Horizon communication medium, providing the ability to send +* and receive messages. Received messages are queued. +* Auth: R. Gariepy +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CLEARPATH_TRANSPORT_H +#define CLEARPATH_TRANSPORT_H + +#include +#include + +#include "husky_base/horizon_legacy/Message.h" +#include "husky_base/horizon_legacy/Exception.h" + +namespace clearpath +{ + + class TransportException : public Exception + { + public: + enum errors + { + ERROR_BASE, + NOT_CONFIGURED, + CONFIGURE_FAIL, + UNACKNOWLEDGED_SEND, + BAD_ACK_RESULT + }; + public: + enum errors type; + + TransportException(const char *msg, enum errors ex_type = ERROR_BASE); + }; + + class BadAckException : public TransportException + { + public: + enum ackFlags + { + BAD_CHECKSUM = 0x01, + BAD_TYPE = 0x02, + BAD_FORMAT = 0x04, + RANGE = 0x08, + NO_BANDWIDTH = 0x10, + OVER_FREQ = 0x20, + OVER_SUBSCRIBE = 0x40 + } ack_flag; + + BadAckException(unsigned int flag); + }; + +/* + * Transport class + */ + class Transport + { + public: + enum counterTypes + { + GARBLE_BYTES, // bytes with no SOH / bad length + INVALID_MSG, // bad format / CRC wrong + IGNORED_ACK, // ack we didn't care about + QUEUE_FULL, // dropped msg because of overfull queue + NUM_COUNTERS // end of list, not actual counter + }; + static const char *counter_names[NUM_COUNTERS]; // N.B: must be updated with counterTypes + + + private: + bool configured; + void *serial; + int retries; + + static const int RETRY_DELAY_MS = 200; + + std::list rx_queue; + static const size_t MAX_QUEUE_LEN = 10000; + + unsigned long counters[NUM_COUNTERS]; + + private: + Message *rxMessage(); + + Message *getAck(); + + void enqueueMessage(Message *msg); + + int openComm(const char *device); + + int closeComm(); + + void resetCounters(); + + protected: + Transport(); + + ~Transport(); + + public: + static Transport &instance(); + + void configure(const char *device, int retries); + + bool isConfigured() + { + return configured; + } + + int close(); + + void poll(); + + void send(Message *m); + + Message *popNext(); + + Message *popNext(enum MessageTypes type); + + Message *waitNext(double timeout = 0.0); + + Message *waitNext(enum MessageTypes type, double timeout = 0.0); + + void flush(std::list *queue = 0); + + void flush(enum MessageTypes type, std::list *queue = 0); + + unsigned long getCounter(enum counterTypes counter) + { + return counters[counter]; + } + + void printCounters(std::ostream &stream = std::cout); + }; + +}; // namespace clearpath + +#endif // CLEARPATH_TRANSPORT_H + diff --git a/husky_base/include/husky_base/horizon_legacy/clearpath.h b/husky_base/include/husky_base/horizon_legacy/clearpath.h new file mode 100644 index 000000000..efb7df331 --- /dev/null +++ b/husky_base/include/husky_base/horizon_legacy/clearpath.h @@ -0,0 +1,55 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: clearpath.h +* Desc: Includes all CCP headers +* Auth: R. Gariepy, Iain Peet +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef CLEARPATH_H +#define CLEARPATH_H + +#include "husky_base/horizon_legacy/Message.h" +#include "husky_base/horizon_legacy/Message_cmd.h" +#include "husky_base/horizon_legacy/Message_request.h" +#include "husky_base/horizon_legacy/Message_data.h" +#include "husky_base/horizon_legacy/Transport.h" + +#endif // CLEARPATH_H diff --git a/husky_base/include/husky_base/horizon_legacy/crc.h b/husky_base/include/husky_base/horizon_legacy/crc.h new file mode 100644 index 000000000..c641fc240 --- /dev/null +++ b/husky_base/include/husky_base/horizon_legacy/crc.h @@ -0,0 +1,60 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: crc.h +* Desc: Definitions of a 16 bit CRC. Uses a table-based implementation. +* When INIT_VAL=0xFFFF, this is identical to that used on the +* various uCs which implement Horizon +* Auth: R. Gariepy +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef __CRC16_H +#define __CRC16_H + +#include + +/***----------Table-driven crc function----------***/ +/*Inputs: -size of the character array, the CRC of which is being computed */ +/* - the initial value of the register to be used in the calculation */ +/* - a pointer to the first element of said character array */ +/*Outputs: the crc as an unsigned short int */ +uint16_t crc16(int size, int init_val, uint8_t *data); + +#endif diff --git a/husky_base/include/husky_base/horizon_legacy/serial.h b/husky_base/include/husky_base/horizon_legacy/serial.h new file mode 100644 index 000000000..6fd310ffd --- /dev/null +++ b/husky_base/include/husky_base/horizon_legacy/serial.h @@ -0,0 +1,61 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: HorizonProtocol.cpp +* Desc: Generic serial communication functions. Pass in void pointers, let the +* platform-specific implementation do the work. Usually, this should be +* included by (and linked against) windows_serial.cpp or linux_serial.cpp +* Auth: R. Gariepy +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#ifndef SERIAL_H_ +#define SERIAL_H_ + +int OpenSerial(void **handle, const char *port_name); + +int SetupSerial(void *handle); + +int WriteData(void *handle, const char *buffer, int length); + +int ReadData(void *handle, char *buffer, int length); + +int CloseSerial(void *handle); + +#endif /* SERIAL_H_ */ diff --git a/husky_base/include/husky_base/horizon_legacy_wrapper.h b/husky_base/include/husky_base/horizon_legacy_wrapper.h new file mode 100644 index 000000000..0585a4f99 --- /dev/null +++ b/husky_base/include/husky_base/horizon_legacy_wrapper.h @@ -0,0 +1,122 @@ +/** +* +* \author Paul Bovbel +* \copyright Copyright (c) 2014-2015, Clearpath Robotics, Inc. +* +* 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 Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to code@clearpathrobotics.com +* +*/ + +#ifndef HUSKY_BASE_HORIZON_LEGACY_WRAPPER_H +#define HUSKY_BASE_HORIZON_LEGACY_WRAPPER_H + +#include "husky_base/horizon_legacy/clearpath.h" +#include "boost/type_traits/is_base_of.hpp" + +namespace +{ + const uint16_t UNSUBSCRIBE = 0xFFFF; +} + +namespace horizon_legacy +{ + + void connect(std::string port); + + void reconnect(); + + void configureLimits(double max_speed, double max_accel); + + void controlSpeed(double speed_left, double speed_right, double accel_left, double accel_right); + + template + struct Channel + { + + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "T must be a descendant of clearpath::Message" + ); + + static Ptr getLatest(double timeout) + { + T *latest = 0; + + // Iterate over all messages in queue and find the latest + while (T *next = T::popNext()) + { + if (latest) + { + delete latest; + latest = 0; + } + latest = next; + } + + // If no messages found in queue, then poll for timeout until one is received + if (!latest) + { + latest = T::waitNext(timeout); + } + + // If no messages received within timeout, make a request + if (!latest) + { + return requestData(timeout); + } + + return Ptr(latest); + } + + static Ptr requestData(double timeout) + { + T *update = 0; + while (!update) + { + update = T::getUpdate(timeout); + if (!update) + { + reconnect(); + } + } + return Ptr(update); + } + + static void subscribe(double frequency) + { + T::subscribe(frequency); + } + + static void unsubscribe() + { + T::subscribe(UNSUBSCRIBE); + } + + }; + +} // namespace husky_base +#endif // HUSKY_BASE_HORIZON_LEGACY_WRAPPER_H diff --git a/husky_base/include/husky_base/husky_diagnostics.h b/husky_base/include/husky_base/husky_diagnostics.h new file mode 100644 index 000000000..0f942bf7f --- /dev/null +++ b/husky_base/include/husky_base/husky_diagnostics.h @@ -0,0 +1,108 @@ +/** +* +* \author Paul Bovbel +* \copyright Copyright (c) 2014-2015, Clearpath Robotics, Inc. +* +* 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 Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to code@clearpathrobotics.com +* +*/ + +#ifndef HUSKY_BASE_HUSKY_DIAGNOSTICS_H +#define HUSKY_BASE_HUSKY_DIAGNOSTICS_H + +#include "ros/ros.h" +#include "diagnostic_updater/diagnostic_updater.h" +#include "husky_base/horizon_legacy_wrapper.h" +#include "husky_msgs/HuskyStatus.h" + +namespace husky_base +{ + + class HuskySoftwareDiagnosticTask : + public diagnostic_updater::DiagnosticTask + { + public: + explicit HuskySoftwareDiagnosticTask(husky_msgs::HuskyStatus &msg, double target_control_freq); + + void run(diagnostic_updater::DiagnosticStatusWrapper &stat); + + void updateControlFrequency(double frequency); + + private: + void reset(); + + double control_freq_, target_control_freq_; + husky_msgs::HuskyStatus &msg_; + }; + + template + class HuskyHardwareDiagnosticTask : + public diagnostic_updater::DiagnosticTask + { + public: + explicit HuskyHardwareDiagnosticTask(husky_msgs::HuskyStatus &msg); + + void run(diagnostic_updater::DiagnosticStatusWrapper &stat) + { + typename horizon_legacy::Channel::Ptr latest = horizon_legacy::Channel::requestData(1.0); + if (latest) + { + update(stat, latest); + } + } + + void update(diagnostic_updater::DiagnosticStatusWrapper &stat, typename horizon_legacy::Channel::Ptr &status); + + private: + husky_msgs::HuskyStatus &msg_; + }; + + template<> + HuskyHardwareDiagnosticTask::HuskyHardwareDiagnosticTask(husky_msgs::HuskyStatus &msg); + + template<> + HuskyHardwareDiagnosticTask::HuskyHardwareDiagnosticTask(husky_msgs::HuskyStatus &msg); + + template<> + HuskyHardwareDiagnosticTask::HuskyHardwareDiagnosticTask( + husky_msgs::HuskyStatus &msg); + + template<> + void HuskyHardwareDiagnosticTask::update( + diagnostic_updater::DiagnosticStatusWrapper &stat, + horizon_legacy::Channel::Ptr &status); + + template<> + void HuskyHardwareDiagnosticTask::update( + diagnostic_updater::DiagnosticStatusWrapper &stat, + horizon_legacy::Channel::Ptr &status); + + template<> + void HuskyHardwareDiagnosticTask::update( + diagnostic_updater::DiagnosticStatusWrapper &stat, + horizon_legacy::Channel::Ptr &status); + +} // namespace husky_base +#endif // HUSKY_BASE_HUSKY_DIAGNOSTICS_H diff --git a/husky_base/include/husky_base/husky_hardware.h b/husky_base/include/husky_base/husky_hardware.h new file mode 100644 index 000000000..36574dcf2 --- /dev/null +++ b/husky_base/include/husky_base/husky_hardware.h @@ -0,0 +1,117 @@ +/** +* +* \author Paul Bovbel +* \copyright Copyright (c) 2014-2015, Clearpath Robotics, Inc. +* +* 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 Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to code@clearpathrobotics.com +* +*/ + +#ifndef HUSKY_BASE_HUSKY_HARDWARE_H +#define HUSKY_BASE_HUSKY_HARDWARE_H + +#include "husky_base/husky_diagnostics.h" +#include "diagnostic_updater/diagnostic_updater.h" +#include "hardware_interface/joint_state_interface.h" +#include "hardware_interface/joint_command_interface.h" +#include "hardware_interface/robot_hw.h" +#include "ros/ros.h" +#include "sensor_msgs/JointState.h" +#include "husky_msgs/HuskyStatus.h" +#include + +namespace husky_base +{ + + /** + * Class representing Husky hardware, allows for ros_control to modify internal state via joint interfaces + */ + class HuskyHardware : + public hardware_interface::RobotHW + { + public: + HuskyHardware(ros::NodeHandle nh, ros::NodeHandle private_nh, double target_control_freq); + + void updateJointsFromHardware(); + + void writeCommandsToHardware(); + + void updateDiagnostics(); + + void reportLoopDuration(const ros::Duration &duration); + + private: + + void initializeDiagnostics(); + + void resetTravelOffset(); + + void registerControlInterfaces(); + + double linearToAngular(const double &travel) const; + + double angularToLinear(const double &angle) const; + + void limitDifferentialSpeed(double &travel_speed_left, double &travel_speed_right); + + ros::NodeHandle nh_, private_nh_; + + // ROS Control interfaces + hardware_interface::JointStateInterface joint_state_interface_; + hardware_interface::VelocityJointInterface velocity_joint_interface_; + + // Diagnostics + ros::Publisher diagnostic_publisher_; + husky_msgs::HuskyStatus husky_status_msg_; + diagnostic_updater::Updater diagnostic_updater_; + HuskyHardwareDiagnosticTask system_status_task_; + HuskyHardwareDiagnosticTask power_status_task_; + HuskyHardwareDiagnosticTask safety_status_task_; + HuskySoftwareDiagnosticTask software_status_task_; + + // ROS Parameters + double wheel_diameter_, max_accel_, max_speed_; + + double polling_timeout_; + + /** + * Joint structure that is hooked to ros_control's InterfaceManager, to allow control via diff_drive_controller + */ + struct Joint + { + double position; + double position_offset; + double velocity; + double effort; + double velocity_command; + + Joint() : + position(0), velocity(0), effort(0), velocity_command(0) + { } + } joints_[4]; + }; + +} // namespace husky_base +#endif // HUSKY_BASE_HUSKY_HARDWARE_H diff --git a/husky_base/launch/base.launch b/husky_base/launch/base.launch new file mode 100644 index 000000000..c9855ba44 --- /dev/null +++ b/husky_base/launch/base.launch @@ -0,0 +1,60 @@ + + + + + + + + + + + + port: $(arg port) + + control_frequency: 10.0 + diagnostic_frequency: 1.0 + + max_acceleration: 3.0 + max_speed: 1.0 + wheel_diameter: 0.3555 + polling_timeout: 10.0 + + + + + + + + + + + + + + + diff --git a/husky_base/package.xml b/husky_base/package.xml new file mode 100644 index 000000000..68ba22558 --- /dev/null +++ b/husky_base/package.xml @@ -0,0 +1,44 @@ + + + husky_base + 0.2.6 + Clearpath Husky robot driver + + Mike Purvis + Paul Bovbel + + Paul Bovbel + + BSD + + http://ros.org/wiki/husky_base + https://github.com/husky/husky_robot/issues + https://github.com/husky/husky_robot + + catkin + controller_manager + diagnostic_updater + diagnostic_msgs + hardware_interface + husky_msgs + roscpp + roslaunch + roslint + sensor_msgs + + controller_manager + diagnostic_updater + diagnostic_msgs + diagnostic_aggregator + diff_drive_controller + geometry_msgs + hardware_interface + husky_control + husky_msgs + husky_description + roscpp + sensor_msgs + topic_tools + + + diff --git a/husky_base/src/horizon_legacy/Logger.cpp b/husky_base/src/horizon_legacy/Logger.cpp new file mode 100644 index 000000000..9db48585e --- /dev/null +++ b/husky_base/src/horizon_legacy/Logger.cpp @@ -0,0 +1,154 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Logger.cpp +* Desc: Provides the Logger singleton which is used within the Clearpath API +* for log / trace message control +* Auth: Iain Peet +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#include +#include +#include +#include + +using namespace std; + +#include "husky_base/horizon_legacy/Logger.h" + +namespace clearpath +{ + + const char *Logger::levelNames[] = {"ERROR", "EXCEPTION", "WARNING", "INFO", "DETAIL"}; + + void loggerTermHandler(int signum) + { + Logger::instance().close(); + + if ((signum == SIGABRT) || (signum == SIGSEGV)) + { + /* We need to catch these so we can flush out any last messages. + * having done this, probably the most consistent thing to do + * is re-raise the signal. (We certainly don't want to just + * ignore these) */ + signal(signum, SIG_DFL); + kill(getpid(), signum); + } + } + + Logger &Logger::instance() + { + static Logger instance; + return instance; + } + + Logger::Logger() : + enabled(true), + level(WARNING), + stream(&cerr) + { + nullStream = new ofstream("/dev/null"); + } + + Logger::~Logger() + { + close(); + } + + void Logger::close() + { + // The actual output stream is owned by somebody else, we only need to flush it + stream->flush(); + + nullStream->close(); + delete nullStream; + nullStream = 0; + } + + std::ostream &Logger::entry(enum logLevels msg_level, const char *file, int line) + { + if (!enabled) { return *nullStream; } + if (msg_level > this->level) { return *nullStream; } + + /* Construct the log entry tag */ + // Always the level of the message + *stream << levelNames[msg_level]; + // If file/line information is provided, need to print it with brackets: + if (file || (line >= 0)) + { + *stream << " ("; + if (file) { *stream << file; } + // Only want a comma if we have both items + if (file && (line >= 0)) { *stream << ","; } + if (line >= 0) { *stream << line; } + *stream << ")"; + } + *stream << ": "; + return *stream; + } + + void Logger::setEnabled(bool en) + { + enabled = en; + } + + void Logger::setLevel(enum logLevels newLevel) + { + level = newLevel; + } + + void Logger::setStream(ostream *newStream) + { + stream->flush(); + stream = newStream; + } + + void Logger::hookFatalSignals() + { + signal(SIGINT, loggerTermHandler); + signal(SIGTERM, loggerTermHandler); + /* If there's an abort or segfault in Logger.close(), well... + * we're pretty much totally screwed anyway. */ + signal(SIGABRT, loggerTermHandler); + signal(SIGSEGV, loggerTermHandler); + } + +}; // namespace clearpath + diff --git a/husky_base/src/horizon_legacy/Message.cpp b/husky_base/src/horizon_legacy/Message.cpp new file mode 100644 index 000000000..237eeadc4 --- /dev/null +++ b/husky_base/src/horizon_legacy/Message.cpp @@ -0,0 +1,502 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: Message.cpp +* Desc: Definitions of the Message class. This class represents a +* single message which is sent or received from a platform +* Auth: R. Gariepy +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#include +#include +#include +#include "husky_base/horizon_legacy/crc.h" +#include "husky_base/horizon_legacy/Message.h" +#include "husky_base/horizon_legacy/Message_data.h" +#include "husky_base/horizon_legacy/Number.h" +#include "husky_base/horizon_legacy/Transport.h" + +// Conditions on the below to handle compiling for nonstandard hardware +#ifdef LOGGING_AVAIL +#include "husky_base/horizon_legacy/Logger.h" +#endif + +using namespace std; + +namespace clearpath +{ + + MessageException::MessageException(const char *msg, enum errors ex_type) + : Exception(msg), type(ex_type) + { +#ifdef LOGGING_AVAIL + CPR_EXCEPT() << "MessageException "< MAX_MSG_LENGTH) + { + /* If payload is too long, the only recourse we have in constructor + * (other than an abort()) is to truncate silently. */ + total_len = MAX_MSG_LENGTH; + payload_len = MAX_MSG_LENGTH - HEADER_LENGTH - CRC_LENGTH; + } + memset(data, 0, MAX_MSG_LENGTH); + memcpy(data + PAYLOAD_OFST, payload, payload_len); + + /* Fill header */ + data[SOH_OFST] = SOH; + setLength(total_len - 3); + setType(type); + setTimestamp(timestamp); + setFlags(flags); + setVersion(version); + data[STX_OFST] = STX; + + /* Generate checksum */ + uint16_t checksum = crc16(crcOffset(), CRC_INIT_VAL, data); + utob(data + crcOffset(), 2, checksum); + } + + Message::~Message() + { + // nothing to do, actually. + } + + void Message::send() + { + // We will retry up to 3 times if we receive CRC errors + for (int i = 0; i < 2; ++i) + { + try + { + Transport::instance().send(this); + return; + } + catch (BadAckException *ex) + { + // Any bad ack other than bad checksum needs to + // be thrown on + if (ex->ack_flag != BadAckException::BAD_CHECKSUM) + { + throw ex; + } + } + } + + /* Make the final attempt outside the try, so any exception + * just goes straight through */ +#ifdef LOGGING_AVAIL + CPR_WARN() << "Bad checksum twice in a row." << endl; +#endif + Transport::instance().send(this); + } + +/** +* Copies message payload into a provided buffer. +* @param buf The buffer to fill +* @param buf_size Maximum length of buf +* @return number of bytes copied. +*/ + size_t Message::getPayload(void *buf, size_t buf_size) + { + // If we don't have enough space in the buffer, don't even try + if (getPayloadLength() > buf_size) { return 0; } + + memcpy(buf, data + PAYLOAD_OFST, getPayloadLength()); + return getPayloadLength(); + } + +/** +* Get a pointer to the payload withing this Message's internal storage. +* @param offset The offset from the beginning of the payload. +* @return a pointer to this Message's internal storage. +*/ + uint8_t *Message::getPayloadPointer(size_t offset) + { + return data + PAYLOAD_OFST + offset; + } + + uint8_t Message::getLength() + { + return data[LENGTH_OFST]; + } + + uint8_t Message::getLengthComp() + { + return data[LENGTH_COMP_OFST]; + } + + uint8_t Message::getVersion() + { + return data[VERSION_OFST]; + } + + uint32_t Message::getTimestamp() + { + return btou(data + TIMESTAMP_OFST, 4); + } + + uint8_t Message::getFlags() + { + return data[FLAGS_OFST]; + } + + uint16_t Message::getType() + { + return btou(data + TYPE_OFST, 2); + } + + uint16_t Message::getChecksum() + { + return btou(data + crcOffset(), 2); + } + + void Message::setLength(uint8_t len) + { + size_t new_total_len = len + 3; + if (new_total_len > MAX_MSG_LENGTH) { return; } + total_len = new_total_len; + data[LENGTH_OFST] = len; + data[LENGTH_COMP_OFST] = ~len; + } + + void Message::setVersion(uint8_t version) + { + data[VERSION_OFST] = version; + } + + void Message::setTimestamp(uint32_t timestamp) + { + utob(data + TIMESTAMP_OFST, 4, timestamp); + } + + void Message::setFlags(uint8_t flags) + { + data[FLAGS_OFST] = flags; + } + + void Message::setType(uint16_t type) + { + utob(data + TYPE_OFST, 2, type); + } + +/** +* Changes the payload length of the packet. +* Does not update packet len/~len fields or the checksum. Call +* makeValid() to update these fields. +* @param len The new payload length +*/ + void Message::setPayloadLength(uint8_t len) + { + + if (((size_t) (len) + HEADER_LENGTH + CRC_LENGTH) > MAX_MSG_LENGTH) { return; } + total_len = len + HEADER_LENGTH + CRC_LENGTH; + } + +/** +* Set the payload of this message. Modifies the length of the +* message as necessary, as per setPayloadLength. +* @see Message::setPayloadLength() +* @param buf Buffer containing the new payload. +* @param buf_size Length of buf. +*/ + void Message::setPayload(void *buf, size_t buf_size) + { + if ((buf_size + HEADER_LENGTH + CRC_LENGTH) > MAX_MSG_LENGTH) { return; } + setPayloadLength(buf_size); + if (buf_size > getPayloadLength()) { return; } + memcpy(data + PAYLOAD_OFST, buf, buf_size); + } + +/** +* Copy the complete raw content of this message to a buffer. +* @param buf The buffer to copy to +* @param buf_size The maximum length of buf +* @return buf on success, NULL on failure +*/ + size_t Message::toBytes(void *buf, size_t buf_size) + { + // If we don't have enough space in the buffer, don't even try + if (total_len > buf_size) + { + return 0; + } + memcpy(buf, data, total_len); + return total_len; + } + +/** +* Checks the consistency of this message. +* @param whyNot Optionally, a reason for validation failure will be +* written here. +* @param strLen Length of the optional whyNot string +* @return true if the message is valid, false otherwise. +*/ + bool Message::isValid(char *whyNot, size_t strLen) + { + // Check SOH + if (data[SOH_OFST] != SOH) + { + if (whyNot) { strncpy(whyNot, "SOH is not present.", strLen); } + return false; + } + // Check STX + if (data[STX_OFST] != STX) + { + if (whyNot) { strncpy(whyNot, "STX is not present.", strLen); } + return false; + } + // Check length matches complement + if (getLength() != ((~getLengthComp()) & 0xff)) + { + if (whyNot) { strncpy(whyNot, "Length does not match complement.", strLen); } + return false; + } + // Check length is correct + if (getLength() != (total_len - 3)) + { + if (whyNot) { strncpy(whyNot, "Length is wrong.", strLen); } + return false; + } + // Check the CRC + if (crc16(crcOffset(), CRC_INIT_VAL, this->data) != getChecksum()) + { + if (whyNot) { strncpy(whyNot, "CRC is wrong.", strLen); } + return false; + } + return true; + } + +/** +* Sets SOH, STX, length, and checksum so that this message becomes valid. +*/ + void Message::makeValid() + { + data[SOH_OFST] = SOH; + data[STX_OFST] = STX; + data[LENGTH_OFST] = total_len - 3; + data[LENGTH_COMP_OFST] = ~data[LENGTH_OFST]; + uint16_t checksum = crc16(crcOffset(), CRC_INIT_VAL, data); + utob(data + crcOffset(), 2, checksum); + } + + std::ostream &Message::printMessage(std::ostream &stream) + { + stream << "Message" << endl; + stream << "=======" << endl; + stream << "Length : " << (int) (getLength()) << endl; + stream << "~Length : " << (int) (getLengthComp()) << endl; + stream << "Version : " << (int) (getVersion()) << endl; + stream << "Flags : " << hex << (int) (getFlags()) << endl; + stream << "Timestamp: " << dec << getTimestamp() << endl; + stream << "Type : " << hex << (int) (getType()) << endl; + stream << "Checksum : " << hex << (int) (getChecksum()) << endl; + stream << dec; + stream << "Raw : "; + printRaw(stream); + return stream; + } + + void Message::printRaw(std::ostream &stream) + { + stream << hex << uppercase; + for (unsigned int i = 0; i < total_len; i++) + { + stream << static_cast(data[i]) << " "; + } + stream << dec; + stream << endl; + } + +/** +* Instantiates the Message subclass corresponding to the +* type field in raw message data. +* @param input The raw message data to instantiate from +* @param msg_len The length of input. +* @return An instance of the correct Message subclass +*/ + Message *Message::factory(void *input, size_t msg_len) + { + uint16_t type = btou((char *) input + TYPE_OFST, 2); + + switch (type) + { + case DATA_ACCEL: + return new DataPlatformAcceleration(input, msg_len); + + case DATA_ACCEL_RAW: + return new DataRawAcceleration(input, msg_len); + + case DATA_ACKERMANN_SETPTS: + return new DataAckermannOutput(input, msg_len); + + case DATA_CURRENT_RAW: + return new DataRawCurrent(input, msg_len); + + case DATA_PLATFORM_NAME: + return new DataPlatformName(input, msg_len); + + case DATA_DIFF_CTRL_CONSTS: + return new DataDifferentialControl(input, msg_len); + + case DATA_DIFF_WHEEL_SPEEDS: + return new DataDifferentialSpeed(input, msg_len); + + case DATA_DIFF_WHEEL_SETPTS: + return new DataDifferentialOutput(input, msg_len); + + case DATA_DISTANCE_DATA: + return new DataRangefinders(input, msg_len); + + case DATA_DISTANCE_TIMING: + return new DataRangefinderTimings(input, msg_len); + + case DATA_ECHO: + return new DataEcho(input, msg_len); + + case DATA_ENCODER: + return new DataEncoders(input, msg_len); + + case DATA_ENCODER_RAW: + return new DataEncodersRaw(input, msg_len); + + case DATA_FIRMWARE_INFO: + return new DataFirmwareInfo(input, msg_len); + + case DATA_GYRO_RAW: + return new DataRawGyro(input, msg_len); + + case DATA_MAGNETOMETER: + return new DataPlatformMagnetometer(input, msg_len); + + case DATA_MAGNETOMETER_RAW: + return new DataRawMagnetometer(input, msg_len); + + case DATA_MAX_ACCEL: + return new DataMaxAcceleration(input, msg_len); + + case DATA_MAX_SPEED: + return new DataMaxSpeed(input, msg_len); + + case DATA_ORIENT: + return new DataPlatformOrientation(input, msg_len); + + case DATA_ORIENT_RAW: + return new DataRawOrientation(input, msg_len); + + case DATA_PLATFORM_INFO: + return new DataPlatformInfo(input, msg_len); + + case DATA_POWER_SYSTEM: + return new DataPowerSystem(input, msg_len); + + case DATA_PROC_STATUS: + return new DataProcessorStatus(input, msg_len); + + case DATA_ROT_RATE: + return new DataPlatformRotation(input, msg_len); + + case DATA_SAFETY_SYSTEM: + return new DataSafetySystemStatus(input, msg_len); + + case DATA_SYSTEM_STATUS: + return new DataSystemStatus(input, msg_len); + + case DATA_TEMPERATURE_RAW: + return new DataRawTemperature(input, msg_len); + + case DATA_VELOCITY_SETPT: + return new DataVelocity(input, msg_len); + + case DATA_VOLTAGE_RAW: + return new DataRawVoltage(input, msg_len); + + default: + return new Message(input, msg_len); + } // switch getType() + } // factory() + + Message *Message::popNext() + { + return Transport::instance().popNext(); + } + + Message *Message::waitNext(double timeout) + { + return Transport::instance().waitNext(timeout); + } + +}; // namespace clearpath + +std::ostream &operator<<(std::ostream &stream, clearpath::Message &msg) +{ + return msg.printMessage(stream); +} + diff --git a/husky_base/src/horizon_legacy/Message_cmd.cpp b/husky_base/src/horizon_legacy/Message_cmd.cpp new file mode 100644 index 000000000..76aead8c6 --- /dev/null +++ b/husky_base/src/horizon_legacy/Message_cmd.cpp @@ -0,0 +1,377 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS™ +* +* File: Message_cmd.cpp +* Desc: Implements Set Message subclasses. +* Auth: Iain Peet +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#include "husky_base/horizon_legacy/Message_cmd.h" + +#include +#include +#include + +using namespace std; + +#include "husky_base/horizon_legacy/Number.h" +// Conditions on the below to handle compiling for nonstandard hardware +#ifdef LOGGING_AVAIL +#include "husky_base/horizon_legacy/Logger.h" +#endif + +namespace clearpath +{ + + long CmdMessage::total_destroyed = 0; + long CmdMessage::total_sent = 0; + + CmdMessage::~CmdMessage() + { + ++total_destroyed; + if (!is_sent) + { +#ifdef LOGGING_AVAIL + CPR_WARN() << "Command message destroyed without being sent. Type: " + << "0x" << hex << getType() << dec + << ". Total unsent: " << (total_destroyed-total_sent) << endl; +#endif + } + else + { + total_sent++; + } + } + + + CmdProcessorReset::CmdProcessorReset() : CmdMessage() + { + setPayloadLength(2); + utob(getPayloadPointer(), 2, (uint16_t) 0x3A18); + setType(CMD_PROCESSOR_RESET); + makeValid(); + } + + CmdProcessorReset::CmdProcessorReset(const CmdProcessorReset &other) : CmdMessage(other) + { + } + + + CmdRestoreSettings::CmdRestoreSettings(enum restoreFlags flags) : CmdMessage() + { + setPayloadLength(3); + + utob(getPayloadPointer(), 2, (uint16_t) (0x3a18)); + *getPayloadPointer(2) = (uint8_t) (flags); + setType(CMD_RESTORE_SETTINGS); + + makeValid(); + } + + CmdRestoreSettings::CmdRestoreSettings(const CmdRestoreSettings &other) : CmdMessage(other) + { + } + + + CmdStoreSettings::CmdStoreSettings() : CmdMessage() + { + setPayloadLength(2); + + utob(getPayloadPointer(), 2, (uint16_t) (0x3a18)); + setType(CMD_STORE_SETTINGS); + + makeValid(); + } + + CmdStoreSettings::CmdStoreSettings(const CmdStoreSettings &other) : CmdMessage(other) + { + } + + + SetAckermannOutput::SetAckermannOutput(double steer, double throt, double brake) : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(STEERING), 2, steer, 100); + ftob(getPayloadPointer(THROTTLE), 2, throt, 100); + ftob(getPayloadPointer(BRAKE), 2, brake, 100); + + setType(SET_ACKERMANN_SETPT); + makeValid(); + } + + SetAckermannOutput::SetAckermannOutput(const SetAckermannOutput &other) : CmdMessage(other) + { + } + + + SetDifferentialControl::SetDifferentialControl( + double p, + double i, + double d, + double feedfwd, + double stic, + double int_lim) + : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(LEFT_P), 2, p, 100); + ftob(getPayloadPointer(LEFT_I), 2, i, 100); + ftob(getPayloadPointer(LEFT_D), 2, d, 100); + ftob(getPayloadPointer(LEFT_FEEDFWD), 2, feedfwd, 100); + ftob(getPayloadPointer(LEFT_STIC), 2, stic, 100); + ftob(getPayloadPointer(LEFT_INT_LIM), 2, int_lim, 100); + + ftob(getPayloadPointer(RIGHT_P), 2, p, 100); + ftob(getPayloadPointer(RIGHT_I), 2, i, 100); + ftob(getPayloadPointer(RIGHT_D), 2, d, 100); + ftob(getPayloadPointer(RIGHT_FEEDFWD), 2, feedfwd, 100); + ftob(getPayloadPointer(RIGHT_STIC), 2, stic, 100); + ftob(getPayloadPointer(RIGHT_INT_LIM), 2, int_lim, 100); + + setType(SET_DIFF_CTRL_CONSTS); + makeValid(); + } + + SetDifferentialControl::SetDifferentialControl( + double left_p, + double left_i, + double left_d, + double left_feedfwd, + double left_stic, + double left_int_lim, + double right_p, + double right_i, + double right_d, + double right_feedfwd, + double right_stic, + double right_int_lim) + : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(LEFT_P), 2, left_p, 100); + ftob(getPayloadPointer(LEFT_I), 2, left_i, 100); + ftob(getPayloadPointer(LEFT_D), 2, left_d, 100); + ftob(getPayloadPointer(LEFT_FEEDFWD), 2, left_feedfwd, 100); + ftob(getPayloadPointer(LEFT_STIC), 2, left_stic, 100); + ftob(getPayloadPointer(LEFT_INT_LIM), 2, left_int_lim, 100); + + ftob(getPayloadPointer(RIGHT_P), 2, right_p, 100); + ftob(getPayloadPointer(RIGHT_I), 2, right_i, 100); + ftob(getPayloadPointer(RIGHT_D), 2, right_d, 100); + ftob(getPayloadPointer(RIGHT_FEEDFWD), 2, right_feedfwd, 100); + ftob(getPayloadPointer(RIGHT_STIC), 2, right_stic, 100); + ftob(getPayloadPointer(RIGHT_INT_LIM), 2, right_int_lim, 100); + + setType(SET_DIFF_CTRL_CONSTS); + makeValid(); + } + + SetDifferentialControl::SetDifferentialControl(const SetDifferentialControl &other) + : CmdMessage(other) + { + } + + + SetDifferentialOutput::SetDifferentialOutput(double left, double right) : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + ftob(getPayloadPointer(LEFT), 2, left, 100); + ftob(getPayloadPointer(RIGHT), 2, right, 100); + + setType(SET_DIFF_WHEEL_SETPTS); + makeValid(); + } + + SetDifferentialOutput::SetDifferentialOutput(const SetDifferentialOutput &other) + : CmdMessage(other) + { + } + + + SetDifferentialSpeed::SetDifferentialSpeed(double left_speed, double right_speed, + double left_accel, double right_accel) + : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(LEFT_SPEED), 2, left_speed, 100); + ftob(getPayloadPointer(LEFT_ACCEL), 2, left_accel, 100); + ftob(getPayloadPointer(RIGHT_SPEED), 2, right_speed, 100); + ftob(getPayloadPointer(RIGHT_ACCEL), 2, right_accel, 100); + + setType(SET_DIFF_WHEEL_SPEEDS); + makeValid(); + } + + SetDifferentialSpeed::SetDifferentialSpeed(const SetDifferentialSpeed &other) : CmdMessage(other) + { + } + + + SetGear::SetGear(uint8_t gear) : CmdMessage() + { + setPayloadLength(1); + getPayloadPointer()[0] = gear; + setType(SET_GEAR_SETPOINT); + makeValid(); + } + + SetGear::SetGear(const SetGear &other) : CmdMessage(other) + { + } + + + SetMaxAccel::SetMaxAccel(double max_fwd, double max_rev) : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(MAX_FWD), 2, max_fwd, 100); + ftob(getPayloadPointer(MAX_REV), 2, max_rev, 100); + + setType(SET_MAX_ACCEL); + makeValid(); + } + + SetMaxAccel::SetMaxAccel(const SetMaxAccel &other) : CmdMessage(other) + { + } + + + SetMaxSpeed::SetMaxSpeed(double max_fwd, double max_rev) : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(MAX_FWD), 2, max_fwd, 100); + ftob(getPayloadPointer(MAX_REV), 2, max_rev, 100); + + setType(SET_MAX_SPEED); + makeValid(); + } + + SetMaxSpeed::SetMaxSpeed(const SetMaxSpeed &other) : CmdMessage(other) + { + } + + + SetPlatformName::SetPlatformName(const char *name) : CmdMessage() + { + size_t cpy_len = strlen(name); + size_t max_len = MAX_MSG_LENGTH - HEADER_LENGTH - CRC_LENGTH - 1 /* for size field */; + if (cpy_len > max_len) { cpy_len = max_len; } + + setPayloadLength(cpy_len + 1); + getPayloadPointer()[0] = cpy_len; + memcpy(getPayloadPointer(1), name, cpy_len); + + setType(SET_PLATFORM_NAME); + + makeValid(); + } + + SetPlatformName::SetPlatformName(const SetPlatformName &other) : CmdMessage(other) + { + } + + + SetPlatformTime::SetPlatformTime(uint32_t time) : CmdMessage() + { + setPayloadLength(4); + utob(getPayloadPointer(), 4, time); + setType(SET_PLATFORM_TIME); + makeValid(); + } + + SetPlatformTime::SetPlatformTime(const SetPlatformTime &other) : CmdMessage(other) + { + } + + + SetSafetySystem::SetSafetySystem(uint16_t flags) : CmdMessage() + { + setPayloadLength(2); + utob(getPayloadPointer(), 2, flags); + setType(SET_SAFETY_SYSTEM); + makeValid(); + } + + SetSafetySystem::SetSafetySystem(const SetSafetySystem &other) : CmdMessage(other) + { + } + + + SetTurn::SetTurn(double trans, double rad, double accel) : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(TRANSLATIONAL), 2, trans, 100); + ftob(getPayloadPointer(TURN_RADIUS), 2, rad, 100); + ftob(getPayloadPointer(TRANS_ACCEL), 2, accel, 100); + + setType(SET_TURN_SETPT); + makeValid(); + } + + SetTurn::SetTurn(const SetTurn &other) : CmdMessage(other) + { + } + + + SetVelocity::SetVelocity(double trans, double rot, double accel) : CmdMessage() + { + setPayloadLength(PAYLOAD_LEN); + + ftob(getPayloadPointer(TRANSLATIONAL), 2, trans, 100); + ftob(getPayloadPointer(ROTATIONAL), 2, rot, 100); + ftob(getPayloadPointer(TRANS_ACCEL), 2, accel, 100); + + setType(SET_VELOCITY_SETPT); + makeValid(); + } + + SetVelocity::SetVelocity(const SetVelocity &other) : CmdMessage(other) + { + } + + +}; // namespace clearpath + diff --git a/husky_base/src/horizon_legacy/Message_data.cpp b/husky_base/src/horizon_legacy/Message_data.cpp new file mode 100644 index 000000000..ceb1f5090 --- /dev/null +++ b/husky_base/src/horizon_legacy/Message_data.cpp @@ -0,0 +1,1150 @@ +#include "husky_base/horizon_legacy/Message_data.h" +#include "husky_base/horizon_legacy/Number.h" +#include "husky_base/horizon_legacy/Transport.h" + +#include +#include +#include +#include + +using namespace std; + +namespace clearpath +{ + +/** +* Macro which generates definitions of the Message constructors +* ExpectedLength is an expression valid within the constructor which gives the +* expected payload length of the message. If the length reported in the message +* header does not match, and exception will be thrown. If ExpectedLength is -1, +* the length check will be skipped. +* NB: Some Messages need to do some extra work in the constructor and don't use +* this macro! +*/ +#define MESSAGE_CONSTRUCTORS(MessageClass, ExpectedLength) \ +MessageClass::MessageClass(void* input, size_t msg_len) : Message(input, msg_len) \ +{ \ + if( ((ExpectedLength) >= 0) && ((ssize_t)getPayloadLength() != (ExpectedLength)) ) { \ + stringstream ss; \ + ss << "Bad payload length: actual="< + +using namespace std; + +namespace clearpath +{ + + void utob(void *dest, size_t dest_len, uint64_t src) + { + size_t i; + /* Copy bytes from int to array */ + for (i = 0; (i < dest_len) && (i < sizeof(uint64_t)); ++i) + { + ((uint8_t *) dest)[i] = (src >> (i * 8)) & 0xff; + } + /* If array is larger than int, 0-fill the remainder */ + for (; i < dest_len; ++i) + { + ((uint8_t *) dest)[i] = 0; + } + } + + void itob(void *dest, size_t dest_len, int64_t src) + { + size_t i; + /* Copy bytes from int to array */ + for (i = 0; (i < dest_len) && (i < sizeof(int64_t)); ++i) + { + ((uint8_t *) dest)[i] = (src >> (i * 8)) & 0xff; + } + /* If array is larger than int, sign-fill the remainder */ + for (; i < dest_len; ++i) + { + if (((uint8_t *) dest)[dest_len - 1] & 0x80) + { // MSB is set, int is negative + ((uint8_t *) dest)[i] = 0xff; + } + else + { // int is positive + ((uint8_t *) dest)[i] = 0; + } + } + } + + void ftob(void *dest, size_t dest_len, double src, double scale) + { + int64_t int_src = (src * scale); + itob(dest, dest_len, int_src); + } + +/* Need to provide all these overloaded functions because integer promotion + * of smaller int types is ambiguous between the uint64/int64 */ + void utob(void *dest, size_t dest_len, uint32_t src) + { + utob(dest, dest_len, (uint64_t) src); + } + + void utob(void *dest, size_t dest_len, uint16_t src) + { + utob(dest, dest_len, (uint64_t) src); + } + + void itob(void *dest, size_t dest_len, int32_t src) + { + itob(dest, dest_len, (int64_t) src); + } + + void itob(void *dest, size_t dest_len, int16_t src) + { + itob(dest, dest_len, (int64_t) src); + } + + uint64_t btou(void *src, size_t src_len) + { + uint64_t retval = 0; + + if (!src_len) { return 0; } + size_t i = src_len - 1; + do + { + retval = retval << 8; + retval |= ((uint8_t *) src)[i]; + } while (i--); + + return retval; + } + + int64_t btoi(void *src, size_t src_len) + { + int64_t retval = 0; + size_t i = sizeof(int64_t); + + if (!src_len) { return 0; } + + /* If array is shorter than int, need to propagate sign bit */ + for (; i >= src_len; --i) + { + retval = retval << 8; + if (((uint8_t *) src)[src_len - 1] & 0x80) + { // MSB is set, int is negative + retval |= 0xff; + } + } + do + { + retval = retval << 8; + retval |= ((uint8_t *) src)[i]; + } while (i--); + + return retval; + } + + double btof(void *src, size_t src_len, double scale) + { + double retval = btoi(src, src_len); + return retval /= scale; + } + +}; // namespace clearpath + diff --git a/husky_base/src/horizon_legacy/Transport.cpp b/husky_base/src/horizon_legacy/Transport.cpp new file mode 100644 index 000000000..9a8d8654b --- /dev/null +++ b/husky_base/src/horizon_legacy/Transport.cpp @@ -0,0 +1,696 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: Transport.cpp +* Desc: Horizon interface class. Class provides the ability to issue +* commands, monitor acknowledgements, and subscribe to data +* Auth: R. Gariepy +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "husky_base/horizon_legacy/Transport.h" +#include "husky_base/horizon_legacy/Number.h" +#include "husky_base/horizon_legacy/Message.h" +#include "husky_base/horizon_legacy/Message_request.h" +#include "husky_base/horizon_legacy/Message_cmd.h" +#include "husky_base/horizon_legacy/serial.h" +#include "husky_base/horizon_legacy/Logger.h" + +using namespace std; + +namespace clearpath +{ + + const char *Transport::counter_names[] = { + "Garbled bytes", + "Invalid messages", + "Ignored acknowledgment", + "Message queue overflow" + }; + + TransportException::TransportException(const char *msg, enum errors ex_type) + : Exception(msg), type(ex_type) + { + if (msg) + { + CPR_EXCEPT() << "TransportException " << (int) type << ": " << message << endl << std::flush; + } + } + + BadAckException::BadAckException(unsigned int flag) : + TransportException(NULL, TransportException::BAD_ACK_RESULT), + ack_flag((enum ackFlags) flag) + { + switch (ack_flag) + { + case BAD_CHECKSUM: + message = "Bad checksum"; + break; + case BAD_TYPE: + message = "Bad message type"; + break; + case BAD_FORMAT: + message = "Bad message format"; + break; + case RANGE: + message = "Range error"; + break; + case OVER_FREQ: + message = "Requested frequency too high"; + break; + case OVER_SUBSCRIBE: + message = "Too many subscriptions"; + break; + default: + message = "Unknown error code."; + break; + }; + stringstream ss; + + CPR_EXCEPT() << "BadAckException (0x" << hex << flag << dec << "): " << message << endl << flush; + } + +#define CHECK_THROW_CONFIGURED() \ + do { \ + if( ! configured ) { \ + throw new TransportException("Transport not configured", TransportException::NOT_CONFIGURED); \ + } \ + } while( 0 ) + +/** +* Transport singleton instance accessor. +* @return The Transport singleton instance. +*/ + Transport &Transport::instance() + { + static Transport instance; + return instance; + } + +/** +* Constructs an unconfigured transport instance. +*/ + Transport::Transport() : + configured(false), + serial(0), + retries(0) + { + for (int i = 0; i < NUM_COUNTERS; ++i) + { + counters[i] = 0; + } + } + + Transport::~Transport() + { + close(); + } + +/** +* Configure this Transport for communication. +* If this Transport is already configured, it will be closed and reconfigured. +* The RX buffer and Message queue will be flushed. +* Counters will be reset. +* @param device The device to communicate over. (Currently, must be serial) +* @param retries Number of times to resend an unacknowledged message. +* @throws TransportException if configuration fails +* @post Transport becomes configured. +*/ + void Transport::configure(const char *device, int retries) + { + if (configured) + { + // Close serial + close(); + } + + // Forget old counters + resetCounters(); + + this->retries = retries; + + if (!openComm(device)) + { + configured = true; + } + else + { + throw new TransportException("Failed to open serial port", TransportException::CONFIGURE_FAIL); + } + } + +/** +* Close this Transport. +* @return Zero on success, nonzero otherwise +* @post Tranport will be unconfigured, regardless of success/failure. +*/ + int Transport::close() + { + int retval = 0; + if (configured) + { + flush(); + retval = closeComm(); + } + configured = false; + return retval; + } + +/** +* Opens a serial port with the default configuration +* (115200 bps, 8-N-1), using the device specified in the constructor +*/ + int Transport::openComm(const char *device) + { + int tmp = OpenSerial(&(this->serial), device); + if (tmp < 0) + { + return -1; + } + tmp = SetupSerial(this->serial); + if (tmp < 0) + { + return -2; + } + return 0; + } + +/** +* Closes the associated serial port +*/ + int Transport::closeComm() + { + CloseSerial(this->serial); + //serial = 0; + return 0; + } + +/** +* Non-blocking message receive function. +* !!! Absolutely not reentrant !!! +* !!! Keeps internal static state !!! +* @return A pointer to a dynamically allocated message, if one has been received +* this call. Null if no complete message has been received. Bad data +* are silently eaten. +*/ + Message *Transport::rxMessage() + { + /* Each time this function is called, any available characters are added + * to the receive buffer. A new Message is created and returned when + * a complete message has been received (the message may be aggregated + * from data received over multiple calls) */ + static char rx_buf[Message::MAX_MSG_LENGTH]; + static size_t rx_inx = 0; + static size_t msg_len = 0; + + if (!rx_inx) { memset(rx_buf, 0xba, Message::MAX_MSG_LENGTH); } + + /* Read in and handle characters, one at a time. + * (This is a simple state machine, using 'rx_inx' as state) */ + while (ReadData(serial, rx_buf + rx_inx, 1) == 1) + { + switch (rx_inx) + { + + /* Waiting for SOH */ + case 0: + if ((uint8_t) (rx_buf[0]) == (uint8_t) (Message::SOH)) + { + rx_inx++; + } + else { counters[GARBLE_BYTES]++; } + break; + + /* Waiting for length */ + case 1: + rx_inx++; + break; + + /* Waiting for ~length */ + case 2: + rx_inx++; + msg_len = rx_buf[1] + 3; + + /* Check for valid length */ + if (static_cast(rx_buf[1] ^ rx_buf[2]) != 0xFF || + (msg_len < Message::MIN_MSG_LENGTH)) + { + counters[GARBLE_BYTES] += rx_inx; + rx_inx = 0; + } + + break; + + //case 9: + //case 10: + // cout << hex << " " << (unsigned int)(rx_buf[rx_inx]); + // if(rx_inx==10) cout << endl; + + /* Waiting for the rest of the message */ + default: + rx_inx++; + if (rx_inx < msg_len) { break; } + /* Finished rxing, reset this state machine and return msg */ + rx_inx = 0; + Message *msg = Message::factory(rx_buf, msg_len); + return msg; + + } // switch( rx_inx ) + } // while( get character ) + + // Breaking out of loop indicates end of available serial input + return NULL; + } + +/** +* Read data until an ack message is found. +* Any data messages received by this function will be queued. +* @return The next ack message, if one is read. +* Null if no ack message has been read yet. +*/ + Message *Transport::getAck() + { + Message *msg = NULL; + + while ((msg = rxMessage())) + { + /* Queue any data messages that turn up */ + if (msg->isData()) + { + enqueueMessage(msg); + continue; + } + + /* Drop invalid messages */ + if (!msg->isValid()) + { + ++counters[INVALID_MSG]; + delete msg; + continue; + } + + return msg; + } + + return NULL; + } + +/** +* Add a Message to the Message queue. +* Checks Message validity, and drops invalid messages. +* Trims queue down to size if it gets too big. +* @param msg The message to enqueue. +*/ + void Transport::enqueueMessage(Message *msg) + { + /* Reject invalid messages */ + if (!msg->isValid()) + { + ++counters[INVALID_MSG]; + delete msg; + return; + } + + // Enqueue + rx_queue.push_back(msg); + + /* Drop the oldest messages if the queue has overflowed */ + while (rx_queue.size() > MAX_QUEUE_LEN) + { + ++counters[QUEUE_FULL]; + delete rx_queue.front(); + rx_queue.pop_front(); + } + } + + +/** +* Public function which makes sure buffered messages are still being read into +* the internal buffer. A compromise between forcing a thread-based implementation +* and blocking on results. This could be placed into a separate thread, but will +* need to be wrapped for thread safety +*/ + void Transport::poll() + { + CHECK_THROW_CONFIGURED(); + + Message *msg = NULL; + + while ((msg = rxMessage())) + { + /* We're not waiting for acks, so drop them */ + if (!msg->isData()) + { + ++counters[IGNORED_ACK]; + delete msg; + continue; + } + + // Message is good, queue it. + enqueueMessage(msg); + } + } + +/** +* Send a message. +* Waits for the firmware to acknowlge and resends the packet +* a few timew if not acknowledged. +* @param m The message to send +* @throw Transport::Exception if never acknowledged. +*/ + void Transport::send(Message *m) + { + CHECK_THROW_CONFIGURED(); + + char skip_send = 0; + Message *ack = NULL; + int transmit_times = 0; + short result_code; + + poll(); + + while (1) + { + // We have exceeded our retry numbers + if (transmit_times > this->retries) + { + break; + } + // Write output + if (!skip_send) { WriteData(serial, (char *) (m->data), m->total_len); } + + // Wait up to 100 ms for ack + for (int i = 0; i < RETRY_DELAY_MS; ++i) + { + usleep(1000); + if ((ack = getAck())) { break; } + } + + // No message - resend + if (ack == NULL) + { + skip_send = 0; + //cout << "No message received yet" << endl; + transmit_times++; + continue; + } + + // Check result code + // If the result code is bad, the message was still transmitted + // successfully + result_code = btou(ack->getPayloadPointer(), 2); + if (result_code > 0) + { + throw new BadAckException(result_code); + } + else + { + // Everything's good - return + break; + } + // Other failure + transmit_times++; + } + if (ack == NULL) + { + throw new TransportException("Unacknowledged send", TransportException::UNACKNOWLEDGED_SEND); + } + delete ack; + + m->is_sent = true; + } + +/** +* Removes the oldest Message from the Message queue and returns it. +* All data waiting in the input buffer will be read and queued. +* @return The oldest message in the queue. This Message is removed +* from the queue. It is dynamically allocated; the caller +* is responsible for freeing it. +* Null if no Messages are currently queued. +*/ + Message *Transport::popNext() + { + CHECK_THROW_CONFIGURED(); + + poll(); // empty the current serial RX queue. + + if (rx_queue.empty()) { return NULL; } + + Message *next = rx_queue.front(); + rx_queue.pop_front(); + return next; + } + +/** +* Finds the oldest message of a specific type in the Message queue, removes +* it, and returns it. Older messages of the wrong type will be left in +* the queue. +* All data waiting in the input buffer will be read and queued. +* @return The oldest message of the correct type in the queue. This Message +* is removed from the queue. It is dynamically allocated; the caller +* is responsible for freeing it. +* Null if no Messages are currently queued. +*/ + Message *Transport::popNext(enum MessageTypes type) + { + CHECK_THROW_CONFIGURED(); + + poll(); // empty the current RX queue + + Message *next; + list::iterator iter; + for (iter = rx_queue.begin(); iter != rx_queue.end(); ++iter) + { + if ((*iter)->getType() == type) + { + next = *iter; + rx_queue.erase(iter); + return next; + } + } + return NULL; + } + +/** +* Fetch a message, blocking if there are no messages currently available. +* @param timeout Maximum time to block, in seconds. +* Actual resolution is system dependent +* A timeout of 0.0 indicates no timeout. +* @return A message. Null if the timeout elapses. */ + Message *Transport::waitNext(double timeout) + { + CHECK_THROW_CONFIGURED(); + + double elapsed = 0.0; + while (true) + { + /* Return a message if it's turned up */ + poll(); + if (!rx_queue.empty()) { return popNext(); } + + /* Check timeout */ + if ((timeout != 0.0) && (elapsed > timeout)) + { + // If we have a timeout set, and it has elapsed, exit. + return NULL; + } + + // Wait a ms before retry + usleep(1000); + elapsed += 0.001; + } + } + +/** +* Fetch a particular type of message, blocking if one isn't available. +* @param type The type of message to fetch +* @param timeout Maximum time to block, in seconds. +* Actual resolution is system dependent +* A timeout of 0.0 indicates no timeout. +* @return A message of the requested type. Nul if the timeout elapses. +*/ + Message *Transport::waitNext(enum MessageTypes type, double timeout) + { + CHECK_THROW_CONFIGURED(); + + double elapsed = 0.0; + Message *msg; + + while (true) + { + /* Check if the message has turned up + * Since we're blocking, not doing anything useful anyway, it doesn't + * really matter that we're iterating the entire message queue every spin. */ + poll(); + msg = popNext(type); + if (msg) { return msg; } + + /* Check timeout */ + if ((timeout != 0.0) && (elapsed > timeout)) + { + // If a timeout is set and has elapsed, fail out. + return NULL; + } + + // Wait a ms before retry + usleep(1000); + elapsed += 0.001; + } + } + +/** +* Empties the serial buffer and the entire message queue. +* Optionally, the queue may be emptied into a provided list. +* @param queue If not null, the entire message queue will be +* added to this list, oldest first. +* If null, messages will be deleted. +*/ + void Transport::flush(list *queue) + { + CHECK_THROW_CONFIGURED(); + + poll(); // flush serial buffer + + /* Either delete or move all elements in the queue, depending + * on whether a destination list is provided */ + list::iterator iter; + for (iter = rx_queue.begin(); iter != rx_queue.end(); ++iter) + { + if (queue) + { + queue->push_back(*iter); + } + else + { + delete *iter; + } + } + + rx_queue.clear(); + } + +/** +* Empties the serial buffer and removes all messages of a given type +* from the message queue. Optionally, removed messages may be added +* to a provided list. +* @param type The type of message to flush out. +* @param queue If not null, all messages of the correct type will be +* added to this list, oldest first. +* If null, messages of the correct type will be deleted. +*/ + void Transport::flush(enum MessageTypes type, list *queue) + { + CHECK_THROW_CONFIGURED(); + + poll(); + + list::iterator iter = rx_queue.begin(); + while (iter != rx_queue.end()) + { + if ((*iter)->getType() == type) + { + /* Element is of flush type. If there's a destination + * list, move it. Otherwise, destroy it */ + if (queue) + { + queue->push_back(*iter); + } + else + { + delete *iter; + } + // This advances to the next element in the queue: + iter = rx_queue.erase(iter); + } + else + { + // Not interested in this element. Next! + iter++; + } + } + } + +/** +* Prints a nice list of counter values +*/ + void Transport::printCounters(ostream &stream) + { + stream << "Transport Counters" << endl; + stream << "==================" << endl; + + size_t longest_name = 0; + size_t cur_len = 0; + for (int i = 0; i < NUM_COUNTERS; ++i) + { + cur_len = strlen(counter_names[i]); + if (cur_len > longest_name) { longest_name = cur_len; } + } + + for (int i = 0; i < NUM_COUNTERS; ++i) + { + cout.width(longest_name); + cout << left << counter_names[i] << ": " << counters[i] << endl; + } + + cout.width(longest_name); + cout << left << "Queue length" << ": " << rx_queue.size() << endl; + } + +/** +* Wipes out counters +*/ + void Transport::resetCounters() + { + for (int i = 0; i < NUM_COUNTERS; ++i) + { + counters[i] = 0; + } + } + +}; // namespace clearpath + diff --git a/husky_base/src/horizon_legacy/crc.cpp b/husky_base/src/horizon_legacy/crc.cpp new file mode 100644 index 000000000..fda29c08d --- /dev/null +++ b/husky_base/src/horizon_legacy/crc.cpp @@ -0,0 +1,88 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: crc.cpp +* Desc: 16 bit CRC function and lookup table. Uses a table-based implementation. +* When INIT_VAL=0xFFFF, this is identical to that used on the +* various uCs which implement Horizon +* Auth: R. Gariepy +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#include + +//CRC lookup table for polynomial 0x1021 +const uint16_t table[256] = + {0, 4129, 8258, 12387, 16516, 20645, 24774, 28903, 33032, 37161, 41290, 45419, 49548, + 53677, 57806, 61935, 4657, 528, 12915, 8786, 21173, 17044, 29431, 25302, 37689, 33560, + 45947, 41818, 54205, 50076, 62463, 58334, 9314, 13379, 1056, 5121, 25830, 29895, 17572, + 21637, 42346, 46411, 34088, 38153, 58862, 62927, 50604, 54669, 13907, 9842, 5649, 1584, + 30423, 26358, 22165, 18100, 46939, 42874, 38681, 34616, 63455, 59390, 55197, 51132, + 18628, 22757, 26758, 30887, 2112, 6241, 10242, 14371, 51660, 55789, 59790, 63919, + 35144, 39273, 43274, 47403, 23285, 19156, 31415, 27286, 6769, 2640, 14899, 10770, + 56317, 52188, 64447, 60318, 39801, 35672, 47931, 43802, 27814, 31879, 19684, 23749, + 11298, 15363, 3168, 7233, 60846, 64911, 52716, 56781, 44330, 48395, 36200, 40265, + 32407, 28342, 24277, 20212, 15891, 11826, 7761, 3696, 65439, 61374, 57309, 53244, + 48923, 44858, 40793, 36728, 37256, 33193, 45514, 41451, 53516, 49453, 61774, 57711, + 4224, 161, 12482, 8419, 20484, 16421, 28742, 24679, 33721, 37784, 41979, 46042, 49981, + 54044, 58239, 62302, 689, 4752, 8947, 13010, 16949, 21012, 25207, 29270, 46570, 42443, + 38312, 34185, 62830, 58703, 54572, 50445, 13538, 9411, 5280, 1153, 29798, 25671, 21540, + 17413, 42971, 47098, 34713, 38840, 59231, 63358, 50973, 55100, 9939, 14066, 1681, 5808, + 26199, 30326, 17941, 22068, 55628, 51565, 63758, 59695, 39368, 35305, 47498, 43435, 22596, + 18533, 30726, 26663, 6336, 2273, 14466, 10403, 52093, 56156, 60223, 64286, 35833, 39896, + 43963, 48026, 19061, 23124, 27191, 31254, 2801, 6864, 10931, 14994, 64814, 60687, 56684, + 52557, 48554, 44427, 40424, 36297, 31782, 27655, 23652, 19525, 15522, 11395, 7392, 3265, + 61215, 65342, 53085, 57212, 44955, 49082, 36825, 40952, 28183, 32310, 20053, 24180, 11923, + 16050, 3793, 7920}; + + +/***----------Table-driven crc function----------***/ +/*Inputs: -size of the character array, the CRC of which is being computed */ +/* - the initial value of the register to be used in the calculation */ +/* - a pointer to the first element of said character array */ +/*Outputs: the crc as an unsigned short int */ +uint16_t crc16(int size, int init_val, uint8_t *data) +{ + unsigned short int crc = static_cast(init_val); + while (size--) + { + crc = (crc << 8) ^ table[((crc >> 8) ^ *data++) & 0xFF]; + } + return crc; +} diff --git a/husky_base/src/horizon_legacy/linux_serial.cpp b/husky_base/src/horizon_legacy/linux_serial.cpp new file mode 100644 index 000000000..7d04117aa --- /dev/null +++ b/husky_base/src/horizon_legacy/linux_serial.cpp @@ -0,0 +1,174 @@ +/** +* _____ +* / _ \ +* / _/ \ \ +* / / \_/ \ +* / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ +* \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | +* \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | +* \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | +* \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| +* ROBOTICS� +* +* File: linux_serial.cpp +* Desc: Linux-compatible serial commands for linking with generic functions +* defined in serial.h +* Auth: M. Hansen, R. Gariepy +* +* Copyright (c) 2010, Clearpath Robotics, Inc. +* All Rights Reserved +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to skynet@clearpathrobotics.com +* +*/ + +#if !defined(LINUX_SERIAL_H) && !defined(win_x86) +#define LINUX_SERIAL_H + +#include "husky_base/horizon_legacy/serial.h" /* Std. function protos */ +#include /* Standard input/output definitions */ +#include /* String function definitions */ +#include /* UNIX standard function definitions */ +#include /* File control definitions */ +#include /* Error number definitions */ +#include /* POSIX terminal control definitions */ +#include /* Malloc */ +#include + +int OpenSerial(void **handle, const char *port_name) +{ + + int fd; /* File descriptor for the port */ + + fd = open(port_name, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd == -1) + { + fprintf(stderr, "Unable to open %s\n\r", port_name); + return -3; + } + + // Verify it is a serial port + if (!isatty(fd)) + { + close(fd); + fprintf(stderr, "%s is not a serial port\n", port_name); + return -3; + } + + *handle = (int *) malloc(sizeof(int)); + **(int **) handle = fd; + return fd; +} + +int SetupSerial(void *handle) +{ + struct termios options; + + // Get the current options for the port... + tcgetattr(*(int *) handle, &options); + + // 8 bits, 1 stop, no parity + options.c_cflag = 0; + options.c_cflag |= CS8; // 8-bit input + + // Enable the receiver and set local mode... + options.c_cflag |= (CLOCAL | CREAD); + + // Set the baud rates to 115200... + cfsetispeed(&options, B115200); + cfsetospeed(&options, B115200); + + // No input processing + options.c_iflag = 0; + + // No output processing + options.c_oflag = 0; + + // No line processing + options.c_lflag = 0; + + // read timeout + options.c_cc[VMIN] = 0; // non-blocking + options.c_cc[VTIME] = 1; // always return after 0.1 seconds + + // Set the new options for the port... + tcsetattr(*(int *) handle, TCSAFLUSH, &options); + + return 0; +} + +int WriteData(void *handle, const char *buffer, int length) +{ + int n = write(*(int *) handle, buffer, length); + if (n < 0) + { + fprintf(stderr, "Error in serial write\r\n"); + return -1; + } + + // serial port output monitor +//#define TX_DEBUG +#ifdef TX_DEBUG + printf("TX:"); + int i; + for (i=0; i +* \copyright Copyright (c) 2014-2015, Clearpath Robotics, Inc. +* +* 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 Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to code@clearpathrobotics.com +* +*/ + +#include "husky_base/horizon_legacy/clearpath.h" +#include + +namespace +{ + std::string port_; +} + +namespace horizon_legacy +{ + + void reconnect() + { + if (port_.empty()) + { + throw std::logic_error("Can't reconnect when port is not configured"); + } + ROS_INFO_STREAM("Connecting to Husky on port " << port_ << "..."); + clearpath::Transport::instance().configure(port_.c_str(), 3); + ROS_INFO("Connected"); + } + + void connect(std::string port) + { + port_ = port; + reconnect(); + } + + void configureLimits(double max_speed, double max_accel) + { + + bool success = false; + while (!success) + { + try + { + clearpath::SetMaxAccel(max_accel, max_accel).send(); + clearpath::SetMaxSpeed(max_speed, max_speed).send(); + success = true; + } + catch (clearpath::Exception *ex) + { + ROS_ERROR_STREAM("Error configuring velocity and accel limits: " << ex->message); + reconnect(); + } + } + } + + void controlSpeed(double speed_left, double speed_right, double accel_left, double accel_right) + { + bool success = false; + while (!success) + { + try + { + clearpath::SetDifferentialSpeed(speed_left, speed_right, accel_left, accel_right).send(); + success = true; + } + catch (clearpath::Exception *ex) + { + ROS_ERROR_STREAM("Error sending speed and accel command: " << ex->message); + reconnect(); + } + } + } + +} diff --git a/husky_base/src/husky_base.cpp b/husky_base/src/husky_base.cpp new file mode 100644 index 000000000..b9ba20eff --- /dev/null +++ b/husky_base/src/husky_base.cpp @@ -0,0 +1,108 @@ +/** +* +* \author Paul Bovbel +* \copyright Copyright (c) 2014-2015, Clearpath Robotics, Inc. +* +* 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 Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to code@clearpathrobotics.com +* +*/ + +#include "husky_base/husky_hardware.h" +#include "controller_manager/controller_manager.h" +#include "ros/callback_queue.h" + +#include + +typedef boost::chrono::steady_clock time_source; + +/** +* Control loop for Husky, not realtime safe +*/ +void controlLoop(husky_base::HuskyHardware &husky, + controller_manager::ControllerManager &cm, + time_source::time_point &last_time) +{ + + // Calculate monotonic time difference + time_source::time_point this_time = time_source::now(); + boost::chrono::duration elapsed_duration = this_time - last_time; + ros::Duration elapsed(elapsed_duration.count()); + last_time = this_time; + + // Process control loop + husky.reportLoopDuration(elapsed); + husky.updateJointsFromHardware(); + cm.update(ros::Time::now(), elapsed); + husky.writeCommandsToHardware(); +} + +/** +* Diagnostics loop for Husky, not realtime safe +*/ +void diagnosticLoop(husky_base::HuskyHardware &husky) +{ + husky.updateDiagnostics(); +} + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "husky_base"); + ros::NodeHandle nh, private_nh("~"); + + double control_frequency, diagnostic_frequency; + private_nh.param("control_frequency", control_frequency, 10.0); + private_nh.param("diagnostic_frequency", diagnostic_frequency, 1.0); + + // Initialize robot hardware and link to controller manager + husky_base::HuskyHardware husky(nh, private_nh, control_frequency); + controller_manager::ControllerManager cm(&husky, nh); + + // Setup separate queue and single-threaded spinner to process timer callbacks + // that interface with Husky hardware - libhorizon_legacy not threadsafe. This + // avoids having to lock around hardware access, but precludes realtime safety + // in the control loop. + ros::CallbackQueue husky_queue; + ros::AsyncSpinner husky_spinner(1, &husky_queue); + + time_source::time_point last_time = time_source::now(); + ros::TimerOptions control_timer( + ros::Duration(1 / control_frequency), + boost::bind(controlLoop, boost::ref(husky), boost::ref(cm), boost::ref(last_time)), + &husky_queue); + ros::Timer control_loop = nh.createTimer(control_timer); + + ros::TimerOptions diagnostic_timer( + ros::Duration(1 / diagnostic_frequency), + boost::bind(diagnosticLoop, boost::ref(husky)), + &husky_queue); + ros::Timer diagnostic_loop = nh.createTimer(diagnostic_timer); + + husky_spinner.start(); + + // Process remainder of ROS callbacks separately, mainly ControlManager related + ros::spin(); + + return 0; +} diff --git a/husky_base/src/husky_diagnostics.cpp b/husky_base/src/husky_diagnostics.cpp new file mode 100644 index 000000000..4cff6de31 --- /dev/null +++ b/husky_base/src/husky_diagnostics.cpp @@ -0,0 +1,256 @@ +/** +* +* \author Paul Bovbel +* \copyright Copyright (c) 2014-2015, Clearpath Robotics, Inc. +* +* 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 Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to code@clearpathrobotics.com +* +*/ + +#include "husky_base/husky_diagnostics.h" + +namespace +{ + const int UNDERVOLT_ERROR = 18; + const int UNDERVOLT_WARN = 19; + const int OVERVOLT_ERROR = 30; + const int OVERVOLT_WARN = 29; + const int DRIVER_OVERTEMP_ERROR = 50; + const int DRIVER_OVERTEMP_WARN = 30; + const int MOTOR_OVERTEMP_ERROR = 80; + const int MOTOR_OVERTEMP_WARN = 70; + const double LOWPOWER_ERROR = 0.2; + const double LOWPOWER_WARN = 0.3; + const int CONTROLFREQ_WARN = 90; + const unsigned int SAFETY_TIMEOUT = 0x1; + const unsigned int SAFETY_LOCKOUT = 0x2; + const unsigned int SAFETY_ESTOP = 0x8; + const unsigned int SAFETY_CCI = 0x10; + const unsigned int SAFETY_PSU = 0x20; + const unsigned int SAFETY_CURRENT = 0x40; + const unsigned int SAFETY_WARN = (SAFETY_TIMEOUT | SAFETY_CCI | SAFETY_PSU); + const unsigned int SAFETY_ERROR = (SAFETY_LOCKOUT | SAFETY_ESTOP | SAFETY_CURRENT); +} // namespace + +namespace husky_base +{ + + template<> + HuskyHardwareDiagnosticTask::HuskyHardwareDiagnosticTask(husky_msgs::HuskyStatus &msg) + : + DiagnosticTask("system_status"), + msg_(msg) + { } + + template<> + void HuskyHardwareDiagnosticTask::update( + diagnostic_updater::DiagnosticStatusWrapper &stat, + horizon_legacy::Channel::Ptr &system_status) + { + msg_.uptime = system_status->getUptime(); + + msg_.battery_voltage = system_status->getVoltage(0); + msg_.left_driver_voltage = system_status->getVoltage(1); + msg_.right_driver_voltage = system_status->getVoltage(2); + + msg_.mcu_and_user_port_current = system_status->getCurrent(0); + msg_.left_driver_current = system_status->getCurrent(1); + msg_.right_driver_current = system_status->getCurrent(2); + + msg_.left_driver_temp = system_status->getTemperature(0); + msg_.right_driver_temp = system_status->getTemperature(1); + msg_.left_motor_temp = system_status->getTemperature(2); + msg_.right_motor_temp = system_status->getTemperature(3); + + stat.add("Uptime", msg_.uptime); + + stat.add("Battery Voltage", msg_.battery_voltage); + stat.add("Left Motor Driver Voltage", msg_.left_driver_voltage); + stat.add("Right Motor Driver Voltage", msg_.right_driver_voltage); + + stat.add("MCU and User Port Current", msg_.mcu_and_user_port_current); + stat.add("Left Motor Driver Current", msg_.left_driver_current); + stat.add("Right Motor Driver Current", msg_.right_driver_current); + + stat.add("Left Motor Driver Temp (C)", msg_.left_driver_temp); + stat.add("Right Motor Driver Temp (C)", msg_.right_driver_temp); + stat.add("Left Motor Temp (C)", msg_.left_motor_temp); + stat.add("Right Motor Temp (C)", msg_.right_motor_temp); + + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "System Status OK"); + if (msg_.battery_voltage > OVERVOLT_ERROR) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Main battery voltage too high"); + } + else if (msg_.battery_voltage > OVERVOLT_WARN) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Main battery voltage too high"); + } + else if (msg_.battery_voltage < UNDERVOLT_ERROR) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Main battery voltage too low"); + } + else if (msg_.battery_voltage < UNDERVOLT_WARN) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Main battery voltage too low"); + } + else + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::OK, "Voltage OK"); + } + + if (std::max(msg_.left_driver_temp, msg_.right_driver_temp) > DRIVER_OVERTEMP_ERROR) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Motor drivers too hot"); + } + else if (std::max(msg_.left_driver_temp, msg_.right_driver_temp) > DRIVER_OVERTEMP_WARN) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Motor drivers too hot"); + } + else if (std::max(msg_.left_motor_temp, msg_.right_motor_temp) > MOTOR_OVERTEMP_ERROR) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Motors too hot"); + } + else if (std::max(msg_.left_motor_temp, msg_.right_motor_temp) > MOTOR_OVERTEMP_WARN) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Motors too hot"); + } + else + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::OK, "Temperature OK"); + } + } + + template<> + HuskyHardwareDiagnosticTask::HuskyHardwareDiagnosticTask(husky_msgs::HuskyStatus &msg) + : + DiagnosticTask("power_status"), + msg_(msg) + { } + + template<> + void HuskyHardwareDiagnosticTask::update( + diagnostic_updater::DiagnosticStatusWrapper &stat, + horizon_legacy::Channel::Ptr &power_status) + { + msg_.charge_estimate = power_status->getChargeEstimate(0); + msg_.capacity_estimate = power_status->getCapacityEstimate(0); + + stat.add("Charge (%)", msg_.charge_estimate); + stat.add("Battery Capacity (Wh)", msg_.capacity_estimate); + + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Power System OK"); + if (msg_.charge_estimate < LOWPOWER_ERROR) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Low power"); + } + else if (msg_.charge_estimate < LOWPOWER_WARN) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Low power"); + } + else + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::OK, "Charge OK"); + } + } + + template<> + HuskyHardwareDiagnosticTask::HuskyHardwareDiagnosticTask( + husky_msgs::HuskyStatus &msg) + : + DiagnosticTask("safety_status"), + msg_(msg) + { } + + template<> + void HuskyHardwareDiagnosticTask::update( + diagnostic_updater::DiagnosticStatusWrapper &stat, + horizon_legacy::Channel::Ptr &safety_status) + { + uint16_t flags = safety_status->getFlags(); + msg_.timeout = (flags & SAFETY_TIMEOUT) > 0; + msg_.lockout = (flags & SAFETY_LOCKOUT) > 0; + msg_.e_stop = (flags & SAFETY_ESTOP) > 0; + msg_.ros_pause = (flags & SAFETY_CCI) > 0; + msg_.no_battery = (flags & SAFETY_PSU) > 0; + msg_.current_limit = (flags & SAFETY_CURRENT) > 0; + + stat.add("Timeout", static_cast(msg_.timeout)); + stat.add("Lockout", static_cast(msg_.lockout)); + stat.add("Emergency Stop", static_cast(msg_.e_stop)); + stat.add("ROS Pause", static_cast(msg_.ros_pause)); + stat.add("No battery", static_cast(msg_.no_battery)); + stat.add("Current limit", static_cast(msg_.current_limit)); + + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Safety System OK"); + if ((flags & SAFETY_ERROR) > 0) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Safety System Error"); + } + else if ((flags & SAFETY_WARN) > 0) + { + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Safety System Warning"); + } + } + + HuskySoftwareDiagnosticTask::HuskySoftwareDiagnosticTask(husky_msgs::HuskyStatus &msg, double target_control_freq) + : + DiagnosticTask("software_status"), + msg_(msg), + target_control_freq_(target_control_freq) + { + reset(); + } + + void HuskySoftwareDiagnosticTask::updateControlFrequency(double frequency) + { + // Keep minimum observed frequency for diagnostics purposes + control_freq_ = std::min(control_freq_, frequency); + } + + void HuskySoftwareDiagnosticTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) + { + msg_.ros_control_loop_freq = control_freq_; + stat.add("ROS Control Loop Frequency", msg_.ros_control_loop_freq); + + double margin = control_freq_ / target_control_freq_ * 100; + + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Software OK"); + if (margin < CONTROLFREQ_WARN) + { + std::ostringstream message; + message << "Control loop executing " << 100 - static_cast(margin) << "% slower than desired"; + stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, message.str()); + } + + reset(); + } + + void HuskySoftwareDiagnosticTask::reset() + { + control_freq_ = std::numeric_limits::infinity(); + target_control_freq_ = 0; + } +} // namespace husky_base diff --git a/husky_base/src/husky_hardware.cpp b/husky_base/src/husky_hardware.cpp new file mode 100644 index 000000000..d2c86f933 --- /dev/null +++ b/husky_base/src/husky_hardware.cpp @@ -0,0 +1,241 @@ +/** +* +* \author Paul Bovbel +* \copyright Copyright (c) 2014-2015, Clearpath Robotics, Inc. +* +* 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 Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. 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. +* +* Please send comments, questions, or patches to code@clearpathrobotics.com +* +*/ + +#include "husky_base/husky_hardware.h" +#include + +namespace +{ + const uint8_t LEFT = 0, RIGHT = 1; +}; + +namespace husky_base +{ + + /** + * Initialize Husky hardware + */ + HuskyHardware::HuskyHardware(ros::NodeHandle nh, ros::NodeHandle private_nh, double target_control_freq) + : + nh_(nh), + private_nh_(private_nh), + system_status_task_(husky_status_msg_), + power_status_task_(husky_status_msg_), + safety_status_task_(husky_status_msg_), + software_status_task_(husky_status_msg_, target_control_freq) + { + private_nh_.param("wheel_diameter", wheel_diameter_, 0.3555); + private_nh_.param("max_accel", max_accel_, 5.0); + private_nh_.param("max_speed", max_speed_, 1.0); + private_nh_.param("polling_timeout_", polling_timeout_, 10.0); + + std::string port; + private_nh_.param("port", port, "/dev/prolific"); + + horizon_legacy::connect(port); + horizon_legacy::configureLimits(max_speed_, max_accel_); + resetTravelOffset(); + initializeDiagnostics(); + registerControlInterfaces(); + } + + /** + * Get current encoder travel offsets from MCU and bias future encoder readings against them + */ + void HuskyHardware::resetTravelOffset() + { + horizon_legacy::Channel::Ptr enc = horizon_legacy::Channel::requestData( + polling_timeout_); + if (enc) + { + for (int i = 0; i < 4; i++) + { + joints_[i].position_offset = linearToAngular(enc->getTravel(i % 2)); + } + } + else + { + ROS_ERROR("Could not get encoder data to calibrate travel offset"); + } + } + + /** + * Register diagnostic tasks with updater class + */ + void HuskyHardware::initializeDiagnostics() + { + horizon_legacy::Channel::Ptr info = + horizon_legacy::Channel::requestData(polling_timeout_); + std::ostringstream hardware_id_stream; + hardware_id_stream << "Husky " << info->getModel() << "-" << info->getSerial(); + + diagnostic_updater_.setHardwareID(hardware_id_stream.str()); + diagnostic_updater_.add(system_status_task_); + diagnostic_updater_.add(power_status_task_); + diagnostic_updater_.add(safety_status_task_); + diagnostic_updater_.add(software_status_task_); + diagnostic_publisher_ = nh_.advertise("status", 10); + } + + + /** + * Register interfaces with the RobotHW interface manager, allowing ros_control operation + */ + void HuskyHardware::registerControlInterfaces() + { + ros::V_string joint_names = boost::assign::list_of("front_left_wheel") + ("front_right_wheel")("rear_left_wheel")("rear_right_wheel"); + for (unsigned int i = 0; i < joint_names.size(); i++) + { + hardware_interface::JointStateHandle joint_state_handle(joint_names[i], + &joints_[i].position, &joints_[i].velocity, + &joints_[i].effort); + joint_state_interface_.registerHandle(joint_state_handle); + + hardware_interface::JointHandle joint_handle( + joint_state_handle, &joints_[i].velocity_command); + velocity_joint_interface_.registerHandle(joint_handle); + } + registerInterface(&joint_state_interface_); + registerInterface(&velocity_joint_interface_); + } + + /** + * External hook to trigger diagnostic update + */ + void HuskyHardware::updateDiagnostics() + { + diagnostic_updater_.force_update(); + husky_status_msg_.header.stamp = ros::Time::now(); + diagnostic_publisher_.publish(husky_status_msg_); + } + + /** + * Pull latest speed and travel measurements from MCU, and store in joint structure for ros_control + */ + void HuskyHardware::updateJointsFromHardware() + { + + horizon_legacy::Channel::Ptr enc = horizon_legacy::Channel::requestData( + polling_timeout_); + if (enc) + { + ROS_DEBUG_STREAM("Received travel information (L:" << enc->getTravel(LEFT) << " R:" << enc->getTravel(RIGHT) << ")"); + for (int i = 0; i < 4; i++) + { + double delta = linearToAngular(enc->getTravel(i % 2)) - joints_[i].position - joints_[i].position_offset; + + // detect suspiciously large readings, possibly from encoder rollover + if (std::abs(delta) < 1.0) + { + joints_[i].position += delta; + } + else + { + // suspicious! drop this measurement and update the offset for subsequent readings + joints_[i].position_offset += delta; + ROS_DEBUG("Dropping overflow measurement from encoder"); + } + } + } + + horizon_legacy::Channel::Ptr speed = horizon_legacy::Channel::requestData( + polling_timeout_); + if (speed) + { + ROS_DEBUG_STREAM("Received linear speed information (L:" << speed->getLeftSpeed() << " R:" << speed->getRightSpeed() << ")"); + for (int i = 0; i < 4; i++) + { + if (i % 2 == LEFT) + { + joints_[i].velocity = linearToAngular(speed->getLeftSpeed()); + } + else + { // assume RIGHT + joints_[i].velocity = linearToAngular(speed->getRightSpeed()); + } + } + } + } + + /** + * Get latest velocity commands from ros_control via joint structure, and send to MCU + */ + void HuskyHardware::writeCommandsToHardware() + { + double diff_speed_left = angularToLinear(joints_[LEFT].velocity_command); + double diff_speed_right = angularToLinear(joints_[RIGHT].velocity_command); + + limitDifferentialSpeed(diff_speed_left, diff_speed_right); + + horizon_legacy::controlSpeed(diff_speed_left, diff_speed_right, max_accel_, max_accel_); + } + + /** + * Update diagnostics with control loop timing information + */ + void HuskyHardware::reportLoopDuration(const ros::Duration &duration) + { + software_status_task_.updateControlFrequency(1 / duration.toSec()); + } + + /** + * Scale left and right speed outputs to maintain ros_control's desired trajectory without saturating the outputs + */ + void HuskyHardware::limitDifferentialSpeed(double &diff_speed_left, double &diff_speed_right) + { + double large_speed = std::max(std::abs(diff_speed_left), std::abs(diff_speed_right)); + + if (large_speed > max_speed_) + { + diff_speed_left *= max_speed_ / large_speed; + diff_speed_right *= max_speed_ / large_speed; + } + } + + /** + * Husky reports travel in metres, need radians for ros_control RobotHW + */ + double HuskyHardware::linearToAngular(const double &travel) const + { + return travel / wheel_diameter_ * 2; + } + + /** + * RobotHW provides velocity command in rad/s, Husky needs m/s, + */ + double HuskyHardware::angularToLinear(const double &angle) const + { + return angle * wheel_diameter_ / 2; + } + + +} // namespace husky_base diff --git a/husky_bringup/CHANGELOG.rst b/husky_bringup/CHANGELOG.rst new file mode 100644 index 000000000..fc4f38ff9 --- /dev/null +++ b/husky_bringup/CHANGELOG.rst @@ -0,0 +1,102 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package husky_bringup +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.6 (2016-10-03) +------------------ +* Adding support for the UM7 IMU. +* Added new ur_modern_driver +* Added param for laser frame_id. +* Contributors: TheDash, Tony Baltovski + +0.2.5 (2015-12-31) +------------------ + +0.2.4 (2015-07-08) +------------------ +* Fix laser path +* Contributors: Paul Bovbel + +0.2.3 (2015-04-08) +------------------ +* Integrate husky_customization workflow +* Contributors: Paul Bovbel + +0.2.2 (2015-03-23) +------------------ +* Fix package urls +* Contributors: Paul Bovbel + +0.2.1 (2015-03-23) +------------------ + +0.2.0 (2015-03-23) +------------------ +* Add UR5 bringup +* Contributors: Paul Bovbel, Devon Ash + +0.1.2 (2015-02-12) +------------------ +* Namespace fixes +* Contributors: Paul Bovbel + +0.1.1 (2015-01-30) +------------------ +* Update website and authors +* Add transform to transfer IMU data to base_link frame +* Make ROBOT_NETWORK optional +* Switch to robot_upstart python API +* Switch to debhelper install method for udeb rules +* Switch to env-hook for file storage +* Switch to new calibration method for um6; switch to imu_filter_magwick +* Contributors: Paul Bovbel + +0.1.0 (2015-01-13) +------------------ +* Port to robot_localization, gyro only pending um6 fixes +* changed the launch file to match parameter namespace changes in the imu_compass node +* ported kingfisher compass calibration to husky +* Added Microstrain device condition - Looks for an attached Microstrain device and installs the necessary launch files from the microstrain_config directory. +* Update sick.launch - Fixed binary name +* Change default IP for LIDAR to 192.168.1.14 +* Add launcher for sick LIDAR. +* Added Microstrain launch file and udev rule. +* Contributors: Jeff Schmidt, Mike Purvis, Paul Bovbel, Prasenjit Mukherjee + +0.0.6 (2013-10-12) +------------------ +* Restore leading slash in checking the joystick path. + This was removed by mistake in an earlier commit. + +0.0.5 (2013-10-05) +------------------ +* Acknowledge the ROBOT_SETUP env variable in the install script. + +0.0.4 (2013-10-03) +------------------ +* Remove the other launchfile check until we get a chance to fix the config location issue. +* adding installation of ekf yaml file to install script +* better parameters for husky compass calibration based on standard husky configurations +* combining both ekf launchers into one and relying on a config file to to pick whether we want an outdoor or indoor ekf to start +* allowing the user to scale the gps data if desired +* adding parameter to lock the altitude at 0 +* set invalid covariance value for enu to really high, instead of -1 + +0.0.3 (2013-10-01) +------------------ +* Add sicktoolbox_wrapper in advance of a config for standard LIDARs. +* Parameterize from environment variables the IMU and GPS ports, and network interface to launch from. + +0.0.2 (2013-09-23) +------------------ +* Compass startup and inertial ekf +* adding magnetometer configuration file to husky_bringup +* added static transform to um6 launcher +* Set namespace to navsat, baud rate to 9600. +* Depend on robot_upstart. +* Add automatic launchfile checks. + +0.0.1 (2013-09-13) +------------------ +* Catkinize package. +* First cut of a new install script. diff --git a/husky_bringup/CMakeLists.txt b/husky_bringup/CMakeLists.txt new file mode 100644 index 000000000..74352bca7 --- /dev/null +++ b/husky_bringup/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 2.8.3) +project(husky_bringup) + +find_package(catkin REQUIRED COMPONENTS roslaunch) + +catkin_package() +catkin_add_env_hooks(50.husky_find_mag_config + SHELLS sh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks) + +roslaunch_add_file_check(launch) + +install(PROGRAMS scripts/install scripts/calibrate_compass scripts/compute_calibration + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(DIRECTORY launch udev config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/husky_bringup/config/mag_config_default.yaml b/husky_bringup/config/mag_config_default.yaml new file mode 100644 index 000000000..e19661fe4 --- /dev/null +++ b/husky_bringup/config/mag_config_default.yaml @@ -0,0 +1,4 @@ +use_mag: false +mag_bias_x: 0 +mag_bias_y: 0 +mag_bias_z: 0 diff --git a/husky_bringup/debian/ros-indigo-husky-bringup.udev b/husky_bringup/debian/ros-indigo-husky-bringup.udev new file mode 100644 index 000000000..73ad91aff --- /dev/null +++ b/husky_bringup/debian/ros-indigo-husky-bringup.udev @@ -0,0 +1,2 @@ +# Udev rule for the Prolific Serial-to-USB adapter shipped standard with Husky +SUBSYSTEMS=="usb", ATTRS{manufacturer}=="Prolific*", SYMLINK+="prolific prolific_$attr{devpath}", MODE="0666" diff --git a/husky_bringup/env-hooks/50.husky_find_mag_config.sh b/husky_bringup/env-hooks/50.husky_find_mag_config.sh new file mode 100644 index 000000000..f1476abf7 --- /dev/null +++ b/husky_bringup/env-hooks/50.husky_find_mag_config.sh @@ -0,0 +1,6 @@ +HUSKY_MAG_CONFIG=$(catkin_find --etc --first-only husky_bringup mag_config.yaml 2>/dev/null) +if [ -z "$HUSKY_MAG_CONFIG" ]; then + HUSKY_MAG_CONFIG=$(catkin_find --share --first-only husky_bringup config/mag_config_default.yaml 2>/dev/null) +fi + +export HUSKY_MAG_CONFIG diff --git a/husky_bringup/launch/lms1xx_config/lms1xx.launch b/husky_bringup/launch/lms1xx_config/lms1xx.launch new file mode 100644 index 000000000..fdbebb759 --- /dev/null +++ b/husky_bringup/launch/lms1xx_config/lms1xx.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/husky_bringup/launch/microstrain_config/microstrain.launch b/husky_bringup/launch/microstrain_config/microstrain.launch new file mode 100644 index 000000000..7eaa8cba8 --- /dev/null +++ b/husky_bringup/launch/microstrain_config/microstrain.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/husky_bringup/launch/navsat_config/navsat.launch b/husky_bringup/launch/navsat_config/navsat.launch new file mode 100644 index 000000000..8f7120bbb --- /dev/null +++ b/husky_bringup/launch/navsat_config/navsat.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + magnetic_declination_radians: 0 + roll_offset: 0 + pitch_offset: 0 + yaw_offset: 0 + zero_altitude: false + broadcast_utm_transform: false + + + diff --git a/husky_bringup/launch/um6_config/um6.launch b/husky_bringup/launch/um6_config/um6.launch new file mode 100644 index 000000000..7ec12dc59 --- /dev/null +++ b/husky_bringup/launch/um6_config/um6.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + orientation_stddev: 0.001 + gain: 0.01 + zeta: 0.001 + publish_tf: false + + + + diff --git a/husky_bringup/launch/um7_config/um7.launch b/husky_bringup/launch/um7_config/um7.launch new file mode 100644 index 000000000..f8491e5a8 --- /dev/null +++ b/husky_bringup/launch/um7_config/um7.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + orientation_stddev: 0.001 + gain: 0.01 + zeta: 0.001 + publish_tf: false + + + + diff --git a/husky_bringup/launch/ur5_arm_config/ur5.launch b/husky_bringup/launch/ur5_arm_config/ur5.launch new file mode 100644 index 000000000..eb0f860b2 --- /dev/null +++ b/husky_bringup/launch/ur5_arm_config/ur5.launch @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/husky_bringup/package.xml b/husky_bringup/package.xml new file mode 100644 index 000000000..3bc1cb481 --- /dev/null +++ b/husky_bringup/package.xml @@ -0,0 +1,42 @@ + + + husky_bringup + 0.2.6 + Clearpath Husky installation and integration package + + Mike Purvis + Paul Bovbel + Devon Ash + + Paul Bovbel + + BSD + + http://ros.org/wiki/husky_bringup + https://github.com/husky/husky_robot/issues + https://github.com/husky/husky_robot + + catkin + + roslaunch + + husky_base + husky_control + husky_description + imu_filter_madgwick + + lms1xx + + nmea_comms + nmea_navsat_driver + python-scipy + robot_localization + robot_upstart + tf + tf2_ros + + + + + + diff --git a/husky_bringup/scripts/calibrate_compass b/husky_bringup/scripts/calibrate_compass new file mode 100755 index 000000000..2afac5276 --- /dev/null +++ b/husky_bringup/scripts/calibrate_compass @@ -0,0 +1,68 @@ +#!/bin/bash + +TEMPDIR=$(mktemp -d --tmpdir calibrate_compass.XXXX) +BAG_FILE=${TEMPDIR}/imu_record.bag +CAL_FILE=${TEMPDIR}/mag_config.yaml +CAL_FINAL_PATH=${ROS_ETC_DIR%/*}/husky_bringup +DURATION=60 + +ROSTOPIC_OUTPUT=$(rostopic list) +if [[ "$?" != "0" ]]; then + echo "ROS appears not to be running. Please start ROS service:" + echo "sudo service ros start" + exit 1 +fi + +rospack find rosbag > /dev/null +if [[ "$?" != "0" ]]; then + echo "Unable to find rosbag record. Is the ROS environment set up correctly?" + exit 1 +fi + +rosbag record /tf /imu/data /imu/mag -O $BAG_FILE --duration $DURATION & +ROSBAG_PID=$! +echo "Started rosbag record, duration $DURATION seconds, pid [${ROSBAG_PID}]" + +rostopic pub /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.2}}' -r 15 & > /dev/null +ROSTOPIC_PID=$! +echo "Started motion commands, pid [${ROSTOPIC_PID}]" + +sleep 2 + +echo "Test underway." + +for i in $(seq 0 ${DURATION}); do + echo -en "\rTime remaining: $((${DURATION}-i)) "; + sleep 1 +done +echo + +echo "Shutting down motion command publisher." +kill $ROSTOPIC_PID + +sleep 2 + +echo "Waiting for rosbag to shut down." + +sleep 2 + +echo "Computing magnetic calibration." +rosrun husky_bringup compute_calibration $BAG_FILE $CAL_FILE &> /tmp/compute_output.log +if [[ "$?" != "0" ]]; then + echo "Unable to compute calibration from recorded bag." + echo "Output in /tmp/compute_output.log" + exit 1 +fi + +if [[ -r $CAL_FILE ]]; then + echo "Calibration generated in $CAL_FILE." + read -r -p "Copy calibration to ${CAL_FINAL_PATH}? [Y/n] " response + response=${response,,} # tolower + if [[ $response =~ ^(no|n)$ ]]; then + echo "Skipping." + else + sudo mkdir -p ${CAL_FINAL_PATH} + sudo cp $CAL_FILE ${CAL_FINAL_PATH} + echo "Restart ROS service to begin using saved calibration." + fi +fi diff --git a/husky_bringup/scripts/compute_calibration b/husky_bringup/scripts/compute_calibration new file mode 100755 index 000000000..7973bcd4b --- /dev/null +++ b/husky_bringup/scripts/compute_calibration @@ -0,0 +1,84 @@ +#!/usr/bin/env python + +import rosbag + +import datetime +from tf.msg import tfMessage +from argparse import ArgumentParser +from geometry_msgs.msg import Quaternion +from numpy import mean, array, hypot, diff, convolve, arange, sin, cos, ones, pi, matrix +from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_multiply,quaternion_matrix +import tf + +# Prompt user if scipy is missing. +try: + from scipy import optimize +except ImportError: + print("This script requires scipy be available.") + print("On Ubuntu: sudo apt-get install python-scipy") + exit(1) + +# Plots are optional +try: + from matplotlib import pyplot + from mpl_toolkits.mplot3d import Axes3D +except ImportError: + pyplot = None + +parser = ArgumentParser(description='Process bag file for compass calibration. Pass a bag containing /imu/mag topic, with the compass facing upright, being slowly rotated in a clockwise direction for 30-120 seconds.') +parser.add_argument('bag', metavar='FILE', type=str, help='input bag file') +parser.add_argument('outfile', metavar='OUTFILE', type=str, help='output yaml file', + nargs="?", default="/tmp/calibration.yaml") +parser.add_argument('--plots', type=bool, help='Show plots if matplotlib available.') +args = parser.parse_args() + +if not args.plots: + pyplot = None + +bag = rosbag.Bag(args.bag) + +vecs = [] +for topic, msg, time in bag.read_messages(topics=("/imu/mag",)): + vecs.append((float(msg.vector.x), float(msg.vector.y), float(msg.vector.z))) + +print ("Using " + str(len(vecs)) + " samples.") + +def calc_R(xc, yc): + """ calculate the distance of each 2D points from the center (xc, yc) """ + return hypot(x-xc, y-yc) + +def f_2(c): + """ calculate the algebraic distance between the 2D points and the mean circle centered at c=(xc, yc) """ + Ri = calc_R(*c) + return Ri - Ri.mean() + + +x,y,z = zip(*vecs) +center_estimate = mean(x), mean(y) +center, ier = optimize.leastsq(f_2, center_estimate) +radius = calc_R(*center).mean() +center = (center[0], center[1], mean(z)) + +a = arange(0, 2*pi + pi/50, pi/50) +circle_points = (center[0] + cos(a) * radius, + center[1] + sin(a) * radius, + center[2] * ones(len(a))) + +print("Magnetic circle centered at " + str(center) + ", with radius " + str(radius)) + +with open(args.outfile, "w") as f: + f.write("# Generated from %s on %s.\n" % (args.bag, datetime.date.today())) + f.write("use_mag: true\n") + f.write("mag_bias_x: {:.9f}\n".format(center[0])) + f.write("mag_bias_y: {:.9f}\n".format(center[1])) + f.write("mag_bias_z: {:.9f}\n".format(center[2])) + +print("Calibration file written to " + args.outfile) + +if pyplot: + ax2 = fig.add_subplot(212, projection='3d') + ax2.view_init(elev=80, azim=5) + ax2.scatter(x, y, z) + ax2.plot(*circle_points, c="red") + pyplot.show() + diff --git a/husky_bringup/scripts/install b/husky_bringup/scripts/install new file mode 100755 index 000000000..78a5ce888 --- /dev/null +++ b/husky_bringup/scripts/install @@ -0,0 +1,29 @@ +#!/usr/bin/env python + +import robot_upstart +import os + +j = robot_upstart.Job(name="husky-core", interface=os.environ.get('ROBOT_NETWORK'), workspace_setup=os.environ.get('ROBOT_SETUP')) + +# Stuff to launch on system startup. +j.add(package="husky_base", glob="launch/*") + +if os.path.exists('/dev/clearpath/imu'): + j.add(package="husky_bringup", glob="launch/um6_config/*") + +if os.path.exists('/dev/clearpath/um7'): + j.add(package="husky_bringup", glob="launch/um7_config/*") + +if os.path.exists('/dev/microstrain'): + j.add(package="husky_bringup", glob="launch/microstrain_config/*") + +if os.path.exists('/dev/clearpath/gps'): + j.add(package="husky_bringup", glob="launch/navsat_config/*") + +if os.environ.get('HUSKY_UR5_IP'): + j.add(package="husky_bringup", glob="launch/ur5_arm_config/*") + +if os.environ.get('HUSKY_LMS1XX_IP'): + j.add(package="husky_bringup", glob="launch/lms1xx_config/*") + +j.install() diff --git a/husky_bringup/udev/10-microstrain.rules b/husky_bringup/udev/10-microstrain.rules new file mode 100644 index 000000000..d5000b632 --- /dev/null +++ b/husky_bringup/udev/10-microstrain.rules @@ -0,0 +1 @@ +SUBSYSTEM=="tty", ATTRS{idVendor}=="199b", ATTRS{idProduct}=="3065", SYMLINK="microstrain", MODE="0666" diff --git a/husky_bringup/udev/41-clearpath.rules b/husky_bringup/udev/41-clearpath.rules new file mode 100644 index 000000000..0bc3f5c53 --- /dev/null +++ b/husky_bringup/udev/41-clearpath.rules @@ -0,0 +1 @@ +SUBSYSTEM=="tty", ATTRS{idProduct}=="6001", ATTRS{idVendor}=="0403", ATTRS{product}=="Clearpath Robotics /*", MODE="0666", PROGRAM="/bin/echo '%s{product}'", SYMLINK+="clearpath%c{3}" diff --git a/husky_bringup/udev/41-hokuyo.rules b/husky_bringup/udev/41-hokuyo.rules new file mode 100644 index 000000000..550dedc27 --- /dev/null +++ b/husky_bringup/udev/41-hokuyo.rules @@ -0,0 +1 @@ +SUBSYSTEMS=="usb", ATTRS{manufacturer}=="Hokuyo*", SYMLINK+="hokuyo hokuyo_$attr{devpath}", MODE="0666" diff --git a/husky_bringup/udev/52-ftdi.rules b/husky_bringup/udev/52-ftdi.rules new file mode 100644 index 000000000..21a6af9ed --- /dev/null +++ b/husky_bringup/udev/52-ftdi.rules @@ -0,0 +1 @@ +SUBSYSTEMS=="usb", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", MODE="0666", SYMLINK+="ftdi_%s{serial}" diff --git a/husky_bringup/udev/README.md b/husky_bringup/udev/README.md new file mode 100644 index 000000000..8056a0797 --- /dev/null +++ b/husky_bringup/udev/README.md @@ -0,0 +1,10 @@ +Udev rules +========== + +These udev rules are provided as a reference for some sensors typically installed on the Husky. If you are using an +Clearpath-provided image to setup your husky PC, they should be installed automatically. + +To install them manually, execute: + +sudo cp $(rospack find husky_bringup)/udev/* /etc/udev/rules.d/ +sudo udevadm control --reload-rules && sudo service udev restart && sudo udevadm trigger diff --git a/husky_description/package.xml b/husky_description/package.xml index 663acab5a..81c274c0a 100644 --- a/husky_description/package.xml +++ b/husky_description/package.xml @@ -23,10 +23,8 @@ robot_state_publisher urdf xacro - ur_description lms1xx - diff --git a/husky_desktop/CHANGELOG.rst b/husky_desktop/CHANGELOG.rst new file mode 100644 index 000000000..065cb819b --- /dev/null +++ b/husky_desktop/CHANGELOG.rst @@ -0,0 +1,38 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package husky_desktop +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.2 (2015-04-08) +------------------ + +0.2.1 (2015-03-23) +------------------ +* Fix package urls +* Contributors: Paul Bovbel + +0.2.0 (2015-03-23) +------------------ + + +0.1.1 (2015-02-20) +------------------ +* Update description and docs +* Contributors: Paul Bovbel + +0.1.0 (2015-01-15) +------------------ +* Update descriptions, dependencies, and maintainer +* Add common message dependencies to desktop metapackage. +* Contributors: Mike Purvis, Paul Bovbel + +0.0.3 (2013-10-04) +------------------ +* Set minimum versions of desktop packages. + +0.0.2 (2013-09-29) +------------------ +* Add husky_simulator to the desktop package. + +0.0.1 (2013-09-16) +------------------ +* Metapackages for robot and desktop variants. diff --git a/husky_desktop/CMakeLists.txt b/husky_desktop/CMakeLists.txt new file mode 100644 index 000000000..be55b44dc --- /dev/null +++ b/husky_desktop/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(husky_desktop) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/husky_desktop/package.xml b/husky_desktop/package.xml new file mode 100644 index 000000000..f908076b9 --- /dev/null +++ b/husky_desktop/package.xml @@ -0,0 +1,21 @@ + + + husky_desktop + 0.2.2 + Metapackage for Clearpath Husky visualization software + Paul Bovbel + + BSD + + http://ros.org/wiki/husky_desktop + https://github.com/husky/husky_desktop/issues + https://github.com/husly/husky_desktop + + catkin + husky_viz + husky_msgs + + + + + diff --git a/husky_gazebo/CHANGELOG.rst b/husky_gazebo/CHANGELOG.rst new file mode 100644 index 000000000..65f0ada6a --- /dev/null +++ b/husky_gazebo/CHANGELOG.rst @@ -0,0 +1,83 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package husky_gazebo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.6 (2016-10-26) +------------------ +* spawn_husky.launch: enable to use custom controller files, i.e effort controller +* spawn_husky.launch: support argument to set urdf file and initial pose +* Contributors: Kei Okada + +0.2.5 (2015-12-31) +------------------ +* Removed duplicate SICK laser plugin in husky_gazebo, since husky_description already contains a SICK laser plugin from the lms package. +* Contributors: Peiyi Chen + +0.2.4 (2015-07-08) +------------------ +* Add pointcloud to laserscan config for simulated kinect +* Contributors: Paul Bovbel + +0.2.3 (2015-04-13) +------------------ +* Fix conflict with underlay + When using -z check, underlayed instances of husky_gazebo would override overlays. +* Contributors: Paul Bovbel + +0.2.2 (2015-04-08) +------------------ +* Reduce physics update rate +* Integrate husky_customization workflow +* Contributors: Paul Bovbel + +0.2.1 (2015-03-23) +------------------ +* Fix package urls +* Add missing dependency +* Contributors: Paul Bovbel + +0.2.0 (2015-03-23) +------------------ +* Refactor URDF +* Add UR5 and Kinect simulation +* Contributors: Paul Bovbel, TheDash + +0.1.4 (2015-02-11) +------------------ +* Add missing dependency +* Contributors: Paul Bovbel + +0.1.3 (2015-02-06) +------------------ +* Fix installing +* Contributors: Paul Bovbel + +0.1.2 (2015-01-30) +------------------ +* Update authors +* Add missing dependency +* Reduce sensor range +* Contributors: Paul Bovbel + +0.1.1 (2015-01-14) +------------------ +* Remove multirobot prefixing, experiment later +* Contributors: Paul Bovbel + +0.1.0 (2015-01-13) +------------------ +* Major refactor for indigo: + * All gazebo plugins moved to urdf/description.gazebo.xacro from husky_description + * Ported to ros_control +* Contributors: James Servos, Mike Purvis, Paul Bovbel, Prasenjit Mukherjee, y22ma + +0.0.3 (2013-11-01) +------------------ + +0.0.2 (2013-09-30) +------------------ +* added package installation rules + +0.0.1 (2013-09-29) +------------------ +* Initial release for Hydro. diff --git a/husky_gazebo/CMakeLists.txt b/husky_gazebo/CMakeLists.txt new file mode 100644 index 000000000..c69280405 --- /dev/null +++ b/husky_gazebo/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 2.8.3) +project(husky_gazebo) + +find_package(catkin REQUIRED COMPONENTS roslaunch) + +catkin_package() +catkin_add_env_hooks(50.husky_gazebo + SHELLS sh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks) + +roslaunch_add_file_check(launch) + +install( + DIRECTORY launch worlds urdf + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/husky_gazebo/env-hooks/50.husky_gazebo.sh b/husky_gazebo/env-hooks/50.husky_gazebo.sh new file mode 100644 index 000000000..94f4655b7 --- /dev/null +++ b/husky_gazebo/env-hooks/50.husky_gazebo.sh @@ -0,0 +1 @@ +export HUSKY_GAZEBO_DESCRIPTION=$(catkin_find --without-underlays --first-only husky_gazebo urdf/description.gazebo.xacro 2>/dev/null) diff --git a/husky_gazebo/launch/husky_empty_world.launch b/husky_gazebo/launch/husky_empty_world.launch new file mode 100644 index 000000000..27eedee27 --- /dev/null +++ b/husky_gazebo/launch/husky_empty_world.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/husky_gazebo/launch/husky_playpen.launch b/husky_gazebo/launch/husky_playpen.launch new file mode 100644 index 000000000..078eb7124 --- /dev/null +++ b/husky_gazebo/launch/husky_playpen.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + diff --git a/husky_gazebo/launch/spawn_husky.launch b/husky_gazebo/launch/spawn_husky.launch new file mode 100644 index 000000000..10e368c9d --- /dev/null +++ b/husky_gazebo/launch/spawn_husky.launch @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + target_frame: base_link # Leave empty to output scan in the pointcloud frame + tolerance: 1.0 + min_height: 0.05 + max_height: 1.0 + + angle_min: -0.52 # -30.0*M_PI/180.0 + angle_max: 0.52 # 30.0*M_PI/180.0 + angle_increment: 0.005 # M_PI/360.0 + scan_time: 0.3333 + range_min: 0.45 + range_max: 4.0 + use_inf: true + + # Concurrency level, affects number of pointclouds queued for processing and number of threads used + # 0 : Detect number of cores + # 1 : Single threaded + # 2->inf : Parallelism level + concurrency_level: 1 + + + + + + + + + + diff --git a/husky_gazebo/package.xml b/husky_gazebo/package.xml new file mode 100644 index 000000000..e1fc1362e --- /dev/null +++ b/husky_gazebo/package.xml @@ -0,0 +1,35 @@ + + + husky_gazebo + 0.2.6 + Clearpath Husky Simulator bringup + + Ryan Gariepy + Prasenjit Mukherjee + Paul Bovbel + Devon Ash + + Paul Bovbel + + BSD + + http://ros.org/wiki/husky_gazebo + https://github.com/husky/husky_simulator/issues + https://github.com/husly/husky_simulator + + catkin + roslaunch + controller_manager + gazebo_plugins + gazebo_ros + gazebo_ros_control + hector_gazebo_plugins + husky_control + husky_description + pointcloud_to_laserscan + robot_state_publisher + rostopic + + + + diff --git a/husky_gazebo/urdf/accessories/kinect_camera.gazebo.xacro b/husky_gazebo/urdf/accessories/kinect_camera.gazebo.xacro new file mode 100644 index 000000000..10b7c49f3 --- /dev/null +++ b/husky_gazebo/urdf/accessories/kinect_camera.gazebo.xacro @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + true + 20.0 + + ${60.0*M_PI/180.0} + + R8G8B8 + 640 + 480 + + + 0.05 + 8.0 + + + + ${prefix} + true + 10 + rgb/image_raw + depth/image_raw + depth/points + rgb/camera_info + depth/camera_info + ${prefix}_frame_optical + 0.1 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.4 + + + + + + + diff --git a/husky_gazebo/urdf/description.gazebo.xacro b/husky_gazebo/urdf/description.gazebo.xacro new file mode 100644 index 000000000..9933413dc --- /dev/null +++ b/husky_gazebo/urdf/description.gazebo.xacro @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + / + + + + + + + + diff --git a/husky_gazebo/urdf/husky.gazebo.xacro b/husky_gazebo/urdf/husky.gazebo.xacro new file mode 100644 index 000000000..d539f3afd --- /dev/null +++ b/husky_gazebo/urdf/husky.gazebo.xacro @@ -0,0 +1,95 @@ + + + + + + + + + + + / + 50.0 + base_link + imu/data + 0.005 0.005 0.005 + 0.005 0.005 0.005 + 0.005 0.005 0.005 + 0.005 0.005 0.005 + 0.005 + 0.005 + + + + + + 40 + / + base_link + base_link + navsat/fix + navsat/vel + 49.9 + 8.9 + 0 + 0 + 0.0001 0.0001 0.0001 + + + + + + + true + + + true + + + true + + + true + + + true + + + true + + + true + + + + + + + + + + + diff --git a/husky_gazebo/worlds/clearpath_playpen.world b/husky_gazebo/worlds/clearpath_playpen.world new file mode 100644 index 000000000..ded885237 --- /dev/null +++ b/husky_gazebo/worlds/clearpath_playpen.world @@ -0,0 +1,3469 @@ + + + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + + + 0.01 + 1 + 100 + 0 0 -9.8 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + -8 -10 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + -3 -10 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + 2 -10 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + 7 -10 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + -7.50675 10 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + -2 10 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + 3 10 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + 8 10 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + 9 -4 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + 9 3 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + -9 -6.56962 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + -10 -2 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + -9.55113 3 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + 9 -8 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + 10 7 0 0 -0 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + -9 7.4858 0 0 -0 0 + + + 1 + + + + + model://fire_hydrant/meshes/fire_hydrant.dae + + + 10 + + + + + + + + + + + + + + model://fire_hydrant/meshes/fire_hydrant.dae + + + + + 0 + 0 + + 0 + 0 + 1 + + 6 -6 0 0 -0 0 + + + 1 + + + + + model://fire_hydrant/meshes/fire_hydrant.dae + + + 10 + + + + + + + + + + + + + + model://fire_hydrant/meshes/fire_hydrant.dae + + + + + 0 + 0 + + 0 + 0 + 1 + + -3 4 0 0 -0 0 + + + 1 + + + + + model://fire_hydrant/meshes/fire_hydrant.dae + + + 10 + + + + + + + + + + + + + + model://fire_hydrant/meshes/fire_hydrant.dae + + + + + 0 + 0 + + 0 + 0 + 1 + + -3 -5 0 0 -0 0 + + + + + + + model://dumpster/meshes/dumpster.dae + + + 10 + + + + + + + + + + + + + + model://dumpster/meshes/dumpster.dae + + + + + + + + 0 + 0 + + 0 + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 1 + + 4 5 0 0 -0 0 + 0 + + + + + + + model://dumpster/meshes/dumpster.dae + + + 10 + + + + + + + + + + + + + + model://dumpster/meshes/dumpster.dae + + + + + + + + 0 + 0 + + 0 + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 1 + + -4 -1 0 0 -0 0 + 0 + + + 1 + + + + + model://jersey_barrier/meshes/jersey_barrier.dae + + + + + 0 0 0.5715 0 -0 0 + + + 4.06542 0.3063 1.143 + + + 10 + + + + + + + + + + + + 0 0 0.032258 0 -0 0 + + + 4.06542 0.8107 0.064516 + + + 10 + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 4.06542 0.65 0.1 + + + 10 + + + + + + + + + + + + 0 0 0.2 0 -0 0 + + + 4.06542 0.5 0.1 + + + 10 + + + + + + + + + + + + 0 -0.224 0.2401 0.9 -0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 0.224 0.2401 -0.9 0 0 + + + 4.06542 0.5 0.064516 + + + 10 + + + + + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + 2.45495 0 0 0 -0 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + + 0 + 0 + + 0 + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 1 + + -5.59121 -8 0 0 -0 0 + 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + + 0 + 0 + + 0 + 0 + 1 + + 7 -2.54643 0 0 -0 0 + 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + + 0 + 0 + + 0 + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 1 + + 7 6.55652 0 0 -0 0 + 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + + 0 + 0 + + 0 + 0 + 1 + + 1.45104 -6.51057 0 0 -0 0 + 0 + + + 1 + + + + + 20 20 0.1 + + + 10 + + + + + + + + + + + + 0 + + + 20 20 0.1 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + -0.464139 0 0 0 -0 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + + 0 + 0 + + 0 + 0 + 1 + + -6.5081 4 0 0 -0 0 + 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + + 0 + 0 + + 0 + 0 + 1 + + -6.46924 5 0 0 -0 0 + 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + + 0 + 0 + + 0 + 0 + 1 + + -6.4302 6 0 0 -0 0 + 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + + 0 + 0 + + 0 + 0 + 1 + + -6 7 0 0 -0 0 + 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + + 0 + 0 + + 0 + 0 + 1 + + -5 7 0 0 -0 0 + 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + + 0 + 0 + + 0 + 0 + 1 + + -4 7 0 0 -0 0 + 0 + + + + + 0 0 0.4 0 -0 0 + 500 + + 51.2096 + 51.2096 + 25 + 0 + 0 + 0 + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + 10 + + + + + + + + + + + + + + model://construction_barrel/meshes/construction_barrel.dae + + + + + 0 + 0 + + 0 + 0 + 1 + + -3 7 0 0 -0 0 + 0 + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + 10 + + + + + + + + + + + + + + 10 10 10 + model://construction_cone/meshes/construction_cone.dae + + + + + 0 + 0 + + 0 + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1 + + 0 + 1 + + 8.42887 -5 0 0 -0 0 + 0 + + + 1298 470000000 + 1304 307193893 + 1412023784 735639465 + + 7.00006 -2.54635 0.049999 4e-06 -0 0.000401 + + 7.00006 -2.54635 0.049999 4e-06 -0 0.000401 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 1.45178 -6.51128 0.049999 -1e-06 -3e-06 3.8e-05 + + 1.45178 -6.51128 0.049999 -1e-06 -3e-06 3.8e-05 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -6.50482 3.9826 0.049999 4e-06 1e-06 -0.009616 + + -6.50482 3.9826 0.049999 4e-06 1e-06 -0.009616 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -6.45065 4.99639 0.049999 -1e-06 -2e-06 0.064981 + + -6.45065 4.99639 0.049999 -1e-06 -2e-06 0.064981 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -6.40174 6.01109 0.05 -1e-06 -1e-06 -0.120729 + + -6.40174 6.01109 0.05 -1e-06 -1e-06 -0.120729 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -6.00634 7.00879 0.049999 3e-06 -0 -0.04957 + + -6.00634 7.00879 0.049999 3e-06 -0 -0.04957 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -4.87565 7.00813 0.049999 -1e-06 -3e-06 0.146162 + + -4.87565 7.00813 0.049999 -1e-06 -3e-06 0.146162 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -3.85615 7.02809 0.0897 -0.01028 -0.077931 0.017839 + + -3.85615 7.02809 0.0897 -0.01028 -0.077931 0.017839 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.95979 6.99802 0.049998 -3e-06 -7e-06 -0.017994 + + -2.95979 6.99802 0.049998 -3e-06 -7e-06 -0.017994 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -5.59121 -8 0.05 0 -1e-06 0 + + -5.59121 -8 0.05 0 -1e-06 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 6.99685 6.55732 0.05 -8.27181e-25 1e-06 -0.007012 + + 6.99685 6.55732 0.05 -8.27181e-25 1e-06 -0.007012 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 8.42887 -5 0.049999 0 3e-06 0 + + 8.42887 -5 0.049999 0 3e-06 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 2.0181 8.87221 0.050916 -1.05879e-22 -3e-06 -0.188821 + + 2.0181 8.87221 0.050916 -1.05879e-22 -3e-06 -0.188821 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2.35048 -4.41043 0.050918 0 -0 2.08003 + + -2.35048 -4.41043 0.050918 0 -0 2.08003 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0.076733 -0.110379 0 0 -0 0 + + 0.076733 -0.110379 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 3.18024 -6.51118 0 0 -0 0 + + 3.18024 -6.51118 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -3 4 0 0 -0 0 + + -3 4 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -7.90686 -2.41363 0 0 -0 0 + + -7.90686 -2.41363 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8 -10 0 0 -0 0 + + -8 -10 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -3 -10 0 0 -0 0 + + -3 -10 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 2 -10 0 0 -0 0 + + 2 -10 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -10.0774 -2.27159 0 0 -0 1.56 + + -10.0774 -2.27159 0 0 -0 1.56 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -9.77013 2.34791 0 0 -0 1.72 + + -9.77013 2.34791 0 0 -0 1.72 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10.0879 -7.62159 0 0 -0 1.56 + + 10.0879 -7.62159 0 0 -0 1.56 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 10 7 0 0 -0 1.48 + + 10 7 0 0 -0 1.48 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -9.64482 7.05946 0 0 -0 1.44 + + -9.64482 7.05946 0 0 -0 1.44 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 2.45495 0 0 0 -0 2.36 + + 2.45495 0 0 0 -0 2.36 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 7 -10 0 0 -0 0 + + 7 -10 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -7.50675 10 0 0 -0 0 + + -7.50675 10 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -2 10 0 0 -0 0 + + -2 10 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 3 10 0 0 -0 0 + + 3 10 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 8 10 0 0 -0 0 + + 8 10 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 9.74902 -2.57759 0 0 -0 1.64 + + 9.74902 -2.57759 0 0 -0 1.64 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 9.70141 2.24369 0 0 -0 1.6 + + 9.70141 2.24369 0 0 -0 1.6 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -10.0607 -6.69856 0 0 0 -1.4 + + -10.0607 -6.69856 0 0 0 -1.4 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + + + 16.2337 1.23674 17.6497 0 0.923643 -3.08299 + orbit + + + + diff --git a/husky_robot/CHANGELOG.rst b/husky_robot/CHANGELOG.rst new file mode 100644 index 000000000..b19f910b8 --- /dev/null +++ b/husky_robot/CHANGELOG.rst @@ -0,0 +1,54 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package husky_robot +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.6 (2016-10-03) +------------------ +* Adding support for the UM7 IMU. +* Contributors: Tony Baltovski + +0.2.5 (2015-12-31) +------------------ + +0.2.4 (2015-07-08) +------------------ + +0.2.3 (2015-04-08) +------------------ + +0.2.2 (2015-03-23) +------------------ +* Fix package urls +* Contributors: Paul Bovbel + +0.2.1 (2015-03-23) +------------------ + +0.2.0 (2015-03-23) +------------------ + +0.1.1 (2015-02-20) +------------------ +* Remove navigation +* Update description and docs +* Contributors: Paul Bovbel + +0.1.0 (2015-01-15) +------------------ +* Update descriptions, dependencies, and maintainer +* Contributors: Paul Bovbel + +* RP-264 Update descriptions, dependencies, and maintainer +* Contributors: Paul Bovbel + +0.0.3 (2013-10-04) +------------------ +* Set minimum version for husky_bringup, add husky_navigation. + +0.0.2 (2013-09-29) +------------------ +* No change. + +0.0.1 (2013-09-16) +------------------ +* Metapackages for robot and desktop variants. diff --git a/husky_robot/CMakeLists.txt b/husky_robot/CMakeLists.txt new file mode 100644 index 000000000..a421857c7 --- /dev/null +++ b/husky_robot/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(husky_robot) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/husky_robot/package.xml b/husky_robot/package.xml new file mode 100644 index 000000000..48734ffae --- /dev/null +++ b/husky_robot/package.xml @@ -0,0 +1,21 @@ + + + husky_robot + 0.2.6 + Metapackage for Clearpath Husky robot software + Paul Bovbel + + BSD + + http://ros.org/wiki/husky_robot + https://github.com/husky/husky_robot/issues + https://github.com/husky/husky_robot + + catkin + husky_bringup + husky_base + + + + + diff --git a/husky_simulator/CHANGELOG.rst b/husky_simulator/CHANGELOG.rst new file mode 100644 index 000000000..673f53eb5 --- /dev/null +++ b/husky_simulator/CHANGELOG.rst @@ -0,0 +1,54 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package husky_simulator +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.6 (2016-10-26) +------------------ + +0.2.5 (2015-12-31) +------------------ + +0.2.4 (2015-07-08) +------------------ + +0.2.3 (2015-04-13) +------------------ + +0.2.2 (2015-04-08) +------------------ + +0.2.1 (2015-03-23) +------------------ + +0.2.0 (2015-03-23) +------------------ + +0.1.4 (2015-02-11) +------------------ + +0.1.3 (2015-02-06) +------------------ + +0.1.2 (2015-01-30) +------------------ +* Update authors +* Contributors: Paul Bovbel + +0.1.1 (2015-01-14) +------------------ +* Contributors: Paul Bovbel + +0.1.0 (2015-01-13) +------------------ +* Removed husky_gazebo_plugins in favor of ros_control +* Contributors: Paul Bovbel + +0.0.3 (2013-11-01) +------------------ + +0.0.2 (2013-09-30) +------------------ + +0.0.1 (2013-09-29) +------------------ +* Add husky_simulator metapackage. diff --git a/husky_simulator/CMakeLists.txt b/husky_simulator/CMakeLists.txt new file mode 100644 index 000000000..5b2b8e7e7 --- /dev/null +++ b/husky_simulator/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(husky_simulator) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/husky_simulator/package.xml b/husky_simulator/package.xml new file mode 100644 index 000000000..ef99445dc --- /dev/null +++ b/husky_simulator/package.xml @@ -0,0 +1,21 @@ + + + husky_simulator + 0.2.6 + Metapackage for Clearpath Husky simulation software + + Paul Bovbel + + BSD + + http://ros.org/wiki/husky_simulator + https://github.com/husky/husky_simulator/issues + https://github.com/husly/husky_simulator + + catkin + husky_gazebo + + + + + diff --git a/husky_ur5_moveit_config/.setup_assistant b/husky_ur5_moveit_config/.setup_assistant deleted file mode 100644 index 309ccf84f..000000000 --- a/husky_ur5_moveit_config/.setup_assistant +++ /dev/null @@ -1,8 +0,0 @@ -moveit_setup_assistant_config: - URDF: - package: husky_description - relative_path: urdf/description.xacro - SRDF: - relative_path: config/husky_ur5.srdf - CONFIG: - generated_timestamp: 1457726540891 diff --git a/husky_ur5_moveit_config/CHANGELOG.rst b/husky_ur5_moveit_config/CHANGELOG.rst deleted file mode 100644 index 93791be45..000000000 --- a/husky_ur5_moveit_config/CHANGELOG.rst +++ /dev/null @@ -1,42 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package husky_ur5_moveit_config -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.2.7 (2015-12-31) ------------------- - -0.2.6 (2015-07-08) ------------------- -* Commented out roslaunch check -* Fix unstable tests -* Contributors: TheDash - -0.2.5 (2015-04-16) ------------------- - -0.2.4 (2015-04-13) ------------------- - -0.2.3 (2015-04-08) ------------------- -* Added collisions so the robot can plan -* Contributors: TheDash - -* Added collisions so the robot can plan -* Contributors: TheDash - -0.2.2 (2015-03-23) ------------------- -* Fix package urls -* Add roslaunch build dependency -* Contributors: Paul Bovbel - -0.2.1 (2015-03-23) ------------------- -* remove temp files -* Contributors: Paul Bovbel - -0.2.0 (2015-03-23) ------------------- -* Initial release -* Contributors: Devon Ash diff --git a/husky_ur5_moveit_config/CMakeLists.txt b/husky_ur5_moveit_config/CMakeLists.txt deleted file mode 100644 index 5698ee0b6..000000000 --- a/husky_ur5_moveit_config/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(husky_ur5_moveit_config) - -find_package(catkin REQUIRED) - -catkin_package() - -#roslaunch_add_file_check(launch) - -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/husky_ur5_moveit_config/config/controllers.yaml b/husky_ur5_moveit_config/config/controllers.yaml deleted file mode 100644 index bcbe16f1c..000000000 --- a/husky_ur5_moveit_config/config/controllers.yaml +++ /dev/null @@ -1,6 +0,0 @@ -controller_list: - - name: arm_controller - action_ns: follow_joint_trajectory - default: true - type: FollowJointTrajectory - joints: [ur5_arm_shoulder_pan_joint, ur5_arm_shoulder_lift_joint, ur5_arm_elbow_joint, ur5_arm_wrist_1_joint, ur5_arm_wrist_2_joint, ur5_arm_wrist_3_joint, ee_joint] diff --git a/husky_ur5_moveit_config/config/fake_controllers.yaml b/husky_ur5_moveit_config/config/fake_controllers.yaml deleted file mode 100644 index 644868b00..000000000 --- a/husky_ur5_moveit_config/config/fake_controllers.yaml +++ /dev/null @@ -1,3 +0,0 @@ -controller_list: - - name: fake_manipulator_controller - joints: [ur5_arm_shoulder_pan_joint, ur5_arm_shoulder_lift_joint, ur5_arm_elbow_joint, ur5_arm_wrist_1_joint, ur5_arm_wrist_2_joint, ur5_arm_wrist_3_joint, ee_joint] diff --git a/husky_ur5_moveit_config/config/husky_ur5.srdf b/husky_ur5_moveit_config/config/husky_ur5.srdf deleted file mode 100644 index 60b4e94b0..000000000 --- a/husky_ur5_moveit_config/config/husky_ur5.srdf +++ /dev/null @@ -1,76 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/config/joint_limits.yaml b/husky_ur5_moveit_config/config/joint_limits.yaml deleted file mode 100644 index acec204dd..000000000 --- a/husky_ur5_moveit_config/config/joint_limits.yaml +++ /dev/null @@ -1,34 +0,0 @@ -# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed -# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] -# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] -joint_limits: - ur5_arm_elbow_joint: - has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: false - max_acceleration: 0 - ur5_arm_shoulder_lift_joint: - has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: false - max_acceleration: 0 - ur5_arm_shoulder_pan_joint: - has_velocity_limits: true - max_velocity: 3.15 - has_acceleration_limits: false - max_acceleration: 0 - ur5_arm_wrist_1_joint: - has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: false - max_acceleration: 0 - ur5_arm_wrist_2_joint: - has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: false - max_acceleration: 0 - ur5_arm_wrist_3_joint: - has_velocity_limits: true - max_velocity: 3.2 - has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file diff --git a/husky_ur5_moveit_config/config/kinematics.yaml b/husky_ur5_moveit_config/config/kinematics.yaml deleted file mode 100644 index 74e2b3a87..000000000 --- a/husky_ur5_moveit_config/config/kinematics.yaml +++ /dev/null @@ -1,5 +0,0 @@ -ur5_arm: - kinematics_solver: ur_kinematics/UR5KinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 diff --git a/husky_ur5_moveit_config/config/ompl_planning.yaml b/husky_ur5_moveit_config/config/ompl_planning.yaml deleted file mode 100644 index 0892165ab..000000000 --- a/husky_ur5_moveit_config/config/ompl_planning.yaml +++ /dev/null @@ -1,67 +0,0 @@ -planner_configs: - SBLkConfigDefault: - type: geometric::SBL - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ESTkConfigDefault: - type: geometric::EST - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - LBKPIECEkConfigDefault: - type: geometric::LBKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - BKPIECEkConfigDefault: - type: geometric::BKPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - KPIECEkConfigDefault: - type: geometric::KPIECE - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] - failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 - min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - RRTkConfigDefault: - type: geometric::RRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - RRTConnectkConfigDefault: - type: geometric::RRTConnect - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstarkConfigDefault: - type: geometric::RRTstar - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 - TRRTkConfigDefault: - type: geometric::TRRT - range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - max_states_failed: 10 # when to start increasing temp. default: 10 - temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 - min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 - init_temperature: 10e-6 # initial temperature. default: 10e-6 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 - k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() - PRMkConfigDefault: - type: geometric::PRM - max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstarkConfigDefault: - type: geometric::PRMstar -ur5_arm: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault \ No newline at end of file diff --git a/husky_ur5_moveit_config/config/padding.yaml b/husky_ur5_moveit_config/config/padding.yaml deleted file mode 100644 index 2da05d06e..000000000 --- a/husky_ur5_moveit_config/config/padding.yaml +++ /dev/null @@ -1,2 +0,0 @@ -default_robot_scale: 1.0 -default_robot_padding: 0.07 diff --git a/husky_ur5_moveit_config/config/sensors_rgbd.yaml b/husky_ur5_moveit_config/config/sensors_rgbd.yaml deleted file mode 100644 index 724e6c8b6..000000000 --- a/husky_ur5_moveit_config/config/sensors_rgbd.yaml +++ /dev/null @@ -1,14 +0,0 @@ -sensors: -- sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater - image_topic: camera/depth/image_raw - queue_size: 5 - near_clipping_plane_distance: 0.3 - far_clipping_plane_distance: 5.0 - skip_vertical_pixels: 1 - skip_horizontal_pixels: 1 - shadow_threshold: 0.2 - padding_scale: 4.0 - padding_offset: 0.03 - filtered_cloud_topic: output_cloud - - diff --git a/husky_ur5_moveit_config/launch/default_warehouse_db.launch b/husky_ur5_moveit_config/launch/default_warehouse_db.launch deleted file mode 100644 index 0119e3295..000000000 --- a/husky_ur5_moveit_config/launch/default_warehouse_db.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/demo.launch b/husky_ur5_moveit_config/launch/demo.launch deleted file mode 100644 index 3f3a2999d..000000000 --- a/husky_ur5_moveit_config/launch/demo.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - [/move_group/fake_controller_joint_states] - - - - - - - - - - - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/husky_ur5_moveit_config/launch/fake_moveit_controller_manager.launch.xml deleted file mode 100644 index df0726814..000000000 --- a/husky_ur5_moveit_config/launch/fake_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/husky_ur5_moveit_controller_manager.launch.xml b/husky_ur5_moveit_config/launch/husky_ur5_moveit_controller_manager.launch.xml deleted file mode 100644 index e6497cc1d..000000000 --- a/husky_ur5_moveit_config/launch/husky_ur5_moveit_controller_manager.launch.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/husky_ur5_moveit_sensor_manager.launch.xml b/husky_ur5_moveit_config/launch/husky_ur5_moveit_sensor_manager.launch.xml deleted file mode 100644 index e49db98ce..000000000 --- a/husky_ur5_moveit_config/launch/husky_ur5_moveit_sensor_manager.launch.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/husky_ur5_moveit_config/launch/husky_ur5_planning_execution.launch b/husky_ur5_moveit_config/launch/husky_ur5_planning_execution.launch deleted file mode 100644 index 132aeed28..000000000 --- a/husky_ur5_moveit_config/launch/husky_ur5_planning_execution.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/joystick_control.launch b/husky_ur5_moveit_config/launch/joystick_control.launch deleted file mode 100644 index f74173527..000000000 --- a/husky_ur5_moveit_config/launch/joystick_control.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/move_group.launch b/husky_ur5_moveit_config/launch/move_group.launch deleted file mode 100644 index de5e5ae34..000000000 --- a/husky_ur5_moveit_config/launch/move_group.launch +++ /dev/null @@ -1,69 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/moveit.rviz b/husky_ur5_moveit_config/launch/moveit.rviz deleted file mode 100644 index 11545f6cc..000000000 --- a/husky_ur5_moveit_config/launch/moveit.rviz +++ /dev/null @@ -1,689 +0,0 @@ -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: /base_footprint - 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_footprint - 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/husky_ur5_moveit_config/launch/moveit_rviz.launch b/husky_ur5_moveit_config/launch/moveit_rviz.launch deleted file mode 100644 index e98254cd0..000000000 --- a/husky_ur5_moveit_config/launch/moveit_rviz.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/ompl_planning_pipeline.launch.xml b/husky_ur5_moveit_config/launch/ompl_planning_pipeline.launch.xml deleted file mode 100644 index d84ed1cab..000000000 --- a/husky_ur5_moveit_config/launch/ompl_planning_pipeline.launch.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/planning_context.launch b/husky_ur5_moveit_config/launch/planning_context.launch deleted file mode 100644 index 3d3f71363..000000000 --- a/husky_ur5_moveit_config/launch/planning_context.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/planning_pipeline.launch.xml b/husky_ur5_moveit_config/launch/planning_pipeline.launch.xml deleted file mode 100644 index 1df6b86a6..000000000 --- a/husky_ur5_moveit_config/launch/planning_pipeline.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/run_benchmark_ompl.launch b/husky_ur5_moveit_config/launch/run_benchmark_ompl.launch deleted file mode 100644 index 7916cf09d..000000000 --- a/husky_ur5_moveit_config/launch/run_benchmark_ompl.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/sensor_manager.launch.xml b/husky_ur5_moveit_config/launch/sensor_manager.launch.xml deleted file mode 100644 index 3aaf657ba..000000000 --- a/husky_ur5_moveit_config/launch/sensor_manager.launch.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/setup_assistant.launch b/husky_ur5_moveit_config/launch/setup_assistant.launch deleted file mode 100644 index f8b9d5908..000000000 --- a/husky_ur5_moveit_config/launch/setup_assistant.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/trajectory_execution.launch.xml b/husky_ur5_moveit_config/launch/trajectory_execution.launch.xml deleted file mode 100644 index e7747c072..000000000 --- a/husky_ur5_moveit_config/launch/trajectory_execution.launch.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/warehouse.launch b/husky_ur5_moveit_config/launch/warehouse.launch deleted file mode 100644 index 72d3a3519..000000000 --- a/husky_ur5_moveit_config/launch/warehouse.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/launch/warehouse_settings.launch.xml b/husky_ur5_moveit_config/launch/warehouse_settings.launch.xml deleted file mode 100644 index d11aaeb21..000000000 --- a/husky_ur5_moveit_config/launch/warehouse_settings.launch.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/husky_ur5_moveit_config/package.xml b/husky_ur5_moveit_config/package.xml deleted file mode 100644 index 5733e17b3..000000000 --- a/husky_ur5_moveit_config/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - husky_ur5_moveit_config - 0.2.7 - - An automatically generated package with all the configuration and launch files for using the husky_ur5 with the MoveIt Motion Planning Framework - - Devon Ash - Devon Ash - - BSD - - http://ros.org/wiki/husky_ur5_moveit_config - https://github.com/husky/husky/issues - https://github.com/husly/husky - - catkin - roslaunch - - - - ur_kinematics - moveit_ros_move_group - moveit_planners_ompl - moveit_ros_visualization - joint_state_publisher - robot_state_publisher - xacro - rviz - moveit_setup_assistant - warehouse_ros - moveit_ros_warehouse - moveit_ros_visualization - moveit_ros_benchmarks - - diff --git a/husky_viz/CHANGELOG.rst b/husky_viz/CHANGELOG.rst new file mode 100644 index 000000000..07c4a7819 --- /dev/null +++ b/husky_viz/CHANGELOG.rst @@ -0,0 +1,55 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package husky_viz +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.2 (2015-04-08) +------------------ +* Toggle viz settings +* Integrate husky_customization workflow +* Contributors: Paul Bovbel + +0.2.1 (2015-03-23) +------------------ +* Update robot visualization +* Enable arm visualization by default +* Fix package urls +* Contributors: Paul Bovbel + +0.2.0 (2015-03-23) +------------------ + + +0.1.1 (2015-02-06) +------------------ +* Update authors and web +* Add IMU viz dependency; Update rviz presets +* Restore interactive marker +* Contributors: Paul Bovbel + +0.1.0 (2015-01-14) +------------------ +* Update visualizations for indigo release +* update maintainer and dependencies +* Remove husky_interactive_markers, switch to generic. +* Add front_laser arg to view_model launcher. +* Default fixed frame for the model viewer should be base_link. +* viewing more options for navigation.rviz +* Contributors: Mike Purvis, Paul Bovbel, Prasenjit Mukherjee + +0.0.3 (2013-10-04) +------------------ +* Set version requirement for description pkg. +* Navigation view launch file and rviz file + +0.0.2 (2013-09-29) +------------------ +* Add interactive_markers pkg to dependencies. +* Add rviz files and launch files for view_robot (odom view + interactive marker control) +* Add roslaunch file check. + +0.0.1 (2013-09-11) +------------------ +* Working view_model.launch. +* Catkinize package, add install targets. +* Split off separate husky_viz package. +* Move models directory up to root diff --git a/husky_viz/CMakeLists.txt b/husky_viz/CMakeLists.txt new file mode 100644 index 000000000..5877f128d --- /dev/null +++ b/husky_viz/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.3) +project(husky_viz) + +find_package(catkin REQUIRED COMPONENTS roslaunch) + +catkin_package() + +roslaunch_add_file_check(launch) + +install( + DIRECTORY launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + diff --git a/husky_viz/launch/view_model.launch b/husky_viz/launch/view_model.launch new file mode 100644 index 000000000..ee303b7d2 --- /dev/null +++ b/husky_viz/launch/view_model.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/husky_viz/launch/view_robot.launch b/husky_viz/launch/view_robot.launch new file mode 100644 index 000000000..d958d4e71 --- /dev/null +++ b/husky_viz/launch/view_robot.launch @@ -0,0 +1,4 @@ + + + + diff --git a/husky_viz/package.xml b/husky_viz/package.xml new file mode 100644 index 000000000..b4b7a664e --- /dev/null +++ b/husky_viz/package.xml @@ -0,0 +1,29 @@ + + + husky_viz + 0.2.2 + Visualization configuration for Clearpath Husky + + Ryan Gariepy + Mike Purvis + + Paul Bovbel + + BSD + + http://ros.org/wiki/husky_viz + https://github.com/husky/husky_desktop/issues + https://github.com/husly/husky_desktop + + catkin + + roslaunch + + husky_description + joint_state_publisher + rviz + rviz_imu_plugin + + + + diff --git a/husky_viz/rviz/model.rviz b/husky_viz/rviz/model.rviz new file mode 100644 index 000000000..78d10e1cf --- /dev/null +++ b/husky_viz/rviz/model.rviz @@ -0,0 +1,172 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 625 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + 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 + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + chassis_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_bumper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rear_bumper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + user_rail_link: + 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/MoveCamera + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.28768 + 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.745399 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.61538 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 876 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000018c00000307fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000307000000de00ffffff000000010000010f00000307fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004300000307000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000025800fffffffb0000000800540069006d00650100000000000004500000000000000000000003580000030700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1535 + X: 1911 + Y: 16 diff --git a/husky_viz/rviz/robot.rviz b/husky_viz/rviz/robot.rviz new file mode 100644 index 000000000..0f749833a --- /dev/null +++ b/husky_viz/rviz/robot.rviz @@ -0,0 +1,385 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Sensing1 + - /Navigation1 + - /Navigation1/Exploration1 + Splitter Ratio: 0.494924 + Tree Height: 583 + - 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: LaserScan +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: 30 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_laser: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_bumper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + inertial_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rear_bumper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rear_right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_chassis_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + user_rail_link: + 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 + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: TwistMarker + Show Axes: false + Show Descriptions: false + Show Visual Aids: false + Update Topic: /twist_marker_server/update + Value: true + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 85; 170; 255 + Enabled: true + Keep: 100 + Length: 0.1 + Name: Odometry + Position Tolerance: 0.1 + Topic: /odometry/filtered + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.40593 + Min Value: 0.40593 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 127 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 999999 + Min Color: 0; 0; 0 + Min Intensity: -1.83101e-36 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.03 + Style: Flat Squares + Topic: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /camera/depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Acceleration properties: + Acc. vector alpha: 0.5 + Acc. vector color: 85; 0; 255 + Acc. vector scale: 0.05 + Derotate acceleration: false + Enable acceleration: true + Axes properties: + Axes scale: 1 + Enable axes: true + Box properties: + Box alpha: 1 + Box color: 85; 0; 255 + Enable box: false + x_scale: 0.4 + y_scale: 0.4 + z_scale: 0.4 + Class: rviz_imu_plugin/Imu + Enabled: false + Name: Imu + Topic: /imu/data + Value: false + Enabled: true + Name: Sensing + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /exploration_polygon_marker + Name: Exploration Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: Exploration Costmap + Topic: /explore_server/explore_costmap/costmap + Value: true + Enabled: true + Name: Exploration + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Static Map + Topic: /map + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: Global Costmap + Topic: /move_base/global_costmap/costmap + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: /move_base/local_costmap/costmap + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Global Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /move_base/NavfnROS/plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /move_base/DWAPlannerROS/local_plan + Value: true + Enabled: false + Name: Navigation + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/MoveCamera + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: false + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 17.1298 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.62908 + Y: -0.0834703 + Z: -0.848712 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.9598 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.2204 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 834 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000018c000002ddfc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000002dd000000de00ffffff000000010000010f000002ddfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000002dd000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000025800fffffffb0000000800540069006d00650100000000000004500000000000000000000004ae000002dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1600 + X: 0 + Y: 27