Repository: mit-racecar/racecar Branch: master Commit: b464bf1f8534 Files: 52 Total size: 102.1 KB Directory structure: gitextract_ho3gnwhp/ ├── .gitignore ├── ackermann_cmd_mux/ │ ├── .cproject │ ├── .gitignore │ ├── .project │ ├── .pydevproject │ ├── .settings/ │ │ └── language.settings.xml │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── LICENSE │ ├── cfg/ │ │ └── reload.cfg │ ├── include/ │ │ └── ackermann_cmd_mux/ │ │ ├── ackermann_cmd_mux_nodelet.hpp │ │ ├── ackermann_cmd_subscribers.hpp │ │ └── exceptions.hpp │ ├── launch/ │ │ ├── ackermann_cmd_mux.launch │ │ ├── reconfigure.launch │ │ └── standalone.launch │ ├── package.xml │ ├── param/ │ │ ├── example.yaml │ │ └── reconfigure.yaml │ ├── plugins/ │ │ └── nodelets.xml │ └── src/ │ ├── ackermann_cmd_mux_nodelet.cpp │ ├── ackermann_cmd_subscribers.cpp │ └── throttle_interpolator.py ├── racecar/ │ ├── CMakeLists.txt │ ├── LICENSE │ ├── config/ │ │ └── racecar-v2/ │ │ ├── high_level_mux.yaml │ │ ├── joy_teleop.yaml │ │ ├── low_level_mux.yaml │ │ ├── sensors.yaml │ │ └── vesc.yaml │ ├── launch/ │ │ ├── includes/ │ │ │ ├── common/ │ │ │ │ ├── joy_teleop.launch.xml │ │ │ │ └── sensors.launch.xml │ │ │ ├── racecar-v2/ │ │ │ │ ├── static_transforms.launch.xml │ │ │ │ ├── velodyne.launch.xml │ │ │ │ └── vesc.launch.xml │ │ │ └── racecar-v2-teleop.launch.xml │ │ ├── known_map_localization.launch │ │ ├── mux.launch │ │ ├── replay_bag_file/ │ │ │ ├── replay_bag_file.launch │ │ │ ├── replay_bag_mapping.launch │ │ │ └── replay_bag_with_lidar_processing.launch │ │ └── teleop.launch │ ├── maps/ │ │ ├── basement_hallways_10cm.yaml │ │ ├── basement_hallways_5cm.yml │ │ └── short-course-33.yml │ ├── package.xml │ ├── rviz/ │ │ ├── known_map_localization.rviz │ │ ├── laser_scan_matcher.rviz │ │ └── mapping.rviz │ └── scripts/ │ └── joy_teleop.py ├── racecar-vm.rosinstall └── racecar.rosinstall ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitignore ================================================ # -*- mode: gitignore; -*- *~ \#*\# ================================================ FILE: ackermann_cmd_mux/.cproject ================================================ ================================================ FILE: ackermann_cmd_mux/.gitignore ================================================ /bin /build /lib ================================================ FILE: ackermann_cmd_mux/.project ================================================ ackermann_cmd_mux org.python.pydev.PyDevBuilder org.eclipse.cdt.managedbuilder.core.genmakebuilder clean,full,incremental, ?name? org.eclipse.cdt.make.core.append_environment true org.eclipse.cdt.make.core.autoBuildTarget all org.eclipse.cdt.make.core.buildArguments org.eclipse.cdt.make.core.buildCommand make org.eclipse.cdt.make.core.cleanBuildTarget clean org.eclipse.cdt.make.core.contents org.eclipse.cdt.make.core.activeConfigSettings org.eclipse.cdt.make.core.enableAutoBuild false org.eclipse.cdt.make.core.enableCleanBuild true org.eclipse.cdt.make.core.enableFullBuild true org.eclipse.cdt.make.core.fullBuildTarget all org.eclipse.cdt.make.core.stopOnError true org.eclipse.cdt.make.core.useDefaultBuildCmd true org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder full,incremental, org.eclipse.cdt.core.cnature org.eclipse.cdt.core.ccnature org.eclipse.cdt.managedbuilder.core.managedBuildNature org.eclipse.cdt.managedbuilder.core.ScannerConfigNature org.python.pydev.pythonNature ================================================ FILE: ackermann_cmd_mux/.pydevproject ================================================ Default python 2.7 ================================================ FILE: ackermann_cmd_mux/.settings/language.settings.xml ================================================

================================================ FILE: ackermann_cmd_mux/CHANGELOG.rst ================================================ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package ackermann_cmd_mux ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.6.3 (2014-12-05) ------------------ 0.6.2 (2014-11-30) ------------------ * ackermann_cmd_mux: fixes node handle for output pub to keep backwards compatibility * adds a little launcher restructing for muxer and smoother * Contributors: Marcus Liebhardt 0.6.0 (2014-07-08) ------------------ * updating package informations. remove email for authors. updating maintainer * Contributors: Jihoon Lee 0.5.3 (2014-03-24) ------------------ * Added support for YAML-CPP 0.5+. The new yaml-cpp API removes the "node >> outputvar;" operator, and it has a new way of loading documents. There's no version hint in the library's headers, so I'm getting the version number from pkg-config. This part of the patch is a port of the ones created by @ktossell for map_server and other packages. The new yaml-cpp also does not have FindValue. * Contributors: Scott K Logan 0.5.2 (2013-11-05) ------------------ 0.5.1 (2013-10-14) ------------------ * Unify naming politics for binaries and plugins. 0.5.0 (2013-10-11) ------------------ * Renamed as ackermann_cmd_mux. 0.4.1 (2013-10-08) ------------------ 0.4.0 (2013-08-29) ------------------ * Add bugtracker and repo info URLs. * Changelogs at package level. * License link fixed. 0.3.0 (2013-07-02) ------------------ 0.2.3 (2013-04-15) ------------------ 0.2.2 (2013-02-10) ------------------ 0.2.1 (2013-02-08) ------------------ 0.2.0 (2013-02-07) ------------------ * Catkinized. 0.1.3 (2013-01-08) ------------------ * More generous description. 0.1.2 (2013-01-02) ------------------ * Dynamically reconfigurable. * Upgraded to new groovy plugin formats. * Add reconfigure launcher and parameter file. * Add a dynamic reconfigure script to accept a yaml filename. 0.1.1 (2012-12-21) ------------------ 0.1.0 (2012-12-05) ------------------ * Initial version. ================================================ FILE: ackermann_cmd_mux/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(ackermann_cmd_mux) find_package(catkin REQUIRED COMPONENTS roscpp pluginlib nodelet dynamic_reconfigure ackermann_msgs std_msgs rospy ) # pkg-config support find_package(PkgConfig) pkg_search_module(yaml-cpp REQUIRED yaml-cpp) if(NOT ${yaml-cpp_VERSION} VERSION_LESS "0.5") add_definitions(-DHAVE_NEW_YAMLCPP) endif() # Dynamic reconfigure support generate_dynamic_reconfigure_options(cfg/reload.cfg) catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME}_nodelet CATKIN_DEPENDS roscpp pluginlib nodelet dynamic_reconfigure ackermann_msgs rospy std_msgs DEPENDS yaml-cpp ) include_directories(include ${catkin_INCLUDE_DIRS} ${yaml-cpp_INCLUDE_DIRS}) # Nodelet library add_library(${PROJECT_NAME}_nodelet src/ackermann_cmd_mux_nodelet.cpp src/ackermann_cmd_subscribers.cpp) #add_dependencies(${PROJECT_NAME}_nodelet ackermann_msgs) add_dependencies(${PROJECT_NAME}_nodelet ${PROJECT_NAME}_gencfg) target_link_libraries(${PROJECT_NAME}_nodelet ${catkin_LIBRARIES} ${yaml-cpp_LIBRARIES}) install(TARGETS ${PROJECT_NAME}_nodelet DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) install(DIRECTORY plugins DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install(DIRECTORY param DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) ================================================ FILE: ackermann_cmd_mux/LICENSE ================================================ # Software License Agreement (BSD License) # # Copyright (c) 2012 Yujin Robot, Daniel Stonier, Jorge Santos # 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 Yujin Robot nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. ================================================ FILE: ackermann_cmd_mux/cfg/reload.cfg ================================================ #!/usr/bin/env python2 PACKAGE = "ackermann_cmd_mux" from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("yaml_cfg_file", str_t, 0, "Pathname to a yaml file for re-configuration of the mux", "") # Second arg is node name it will run in (doc purposes only), third is generated filename prefix exit(gen.generate(PACKAGE, "ackermann_cmd_mux_reload", "reload")) ================================================ FILE: ackermann_cmd_mux/include/ackermann_cmd_mux/ackermann_cmd_mux_nodelet.hpp ================================================ /** * @file /include/ackermann_cmd_mux/ackermann_cmd_mux_nodelet.hpp * * @brief Structure for the ackermann_cmd_mux. * * License: BSD * https://raw.github.com/yujinrobot/yujin_ocs/hydro/yocs_ackermann_cmd_mux/LICENSE **/ /***************************************************************************** ** Ifdefs *****************************************************************************/ #ifndef RACECAR_ACKERMANN_CMD_MUX_HPP_ #define RACECAR_ACKERMANN_CMD_MUX_HPP_ /***************************************************************************** ** Includes *****************************************************************************/ #include #include #include #include "ackermann_cmd_mux/reloadConfig.h" #include "ackermann_cmd_mux/ackermann_cmd_subscribers.hpp" /***************************************************************************** ** Namespaces *****************************************************************************/ namespace ackermann_cmd_mux { /***************************************************************************** ** AckermannCmdMux *****************************************************************************/ class AckermannCmdMuxNodelet : public nodelet::Nodelet { public: virtual void onInit(); AckermannCmdMuxNodelet() { dynamic_reconfigure_server = NULL; } ~AckermannCmdMuxNodelet() { if (dynamic_reconfigure_server != NULL) delete dynamic_reconfigure_server; } private: AckermannCmdSubscribers ackermann_cmd_sub; /**< Pool of ackermann_cmd topics subscribers */ ros::Publisher mux_ackermann_cmd_pub; /**< Multiplexed ackermann command topic */ ros::Publisher active_subscriber; /**< Currently allowed ackermann_cmd subscriber */ void timerCallback(const ros::TimerEvent& event, unsigned int idx); void ackermannCmdCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& msg, unsigned int idx); /********************* ** Dynamic Reconfigure **********************/ dynamic_reconfigure::Server * dynamic_reconfigure_server; dynamic_reconfigure::Server::CallbackType dynamic_reconfigure_cb; void reloadConfiguration(ackermann_cmd_mux::reloadConfig &config, uint32_t unused_level); /********************* ** Private Classes **********************/ // Functor assigned to each incoming ackermann command topic to bind it to ackermann_cmd callback class AckermannCmdFunctor { private: unsigned int idx; AckermannCmdMuxNodelet* node; public: AckermannCmdFunctor(unsigned int idx, AckermannCmdMuxNodelet* node) : idx(idx), node(node) { } void operator()(const ackermann_msgs::AckermannDriveStamped::ConstPtr& msg) { node->ackermannCmdCallback(msg, idx); } }; // Functor assigned to each ackermann command messages source to bind it to timer callback class TimerFunctor { private: unsigned int idx; AckermannCmdMuxNodelet* node; public: TimerFunctor(unsigned int idx, AckermannCmdMuxNodelet* node) : idx(idx), node(node) { } void operator()(const ros::TimerEvent& event) { node->timerCallback(event, idx); } }; }; } // namespace ackermann_cmd_mux #endif /* RACECAR_ACKERMANN_CMD_MUX_HPP_ */ ================================================ FILE: ackermann_cmd_mux/include/ackermann_cmd_mux/ackermann_cmd_subscribers.hpp ================================================ /** * @file /include/ackermann_cmd_mux/ackermann_cmd_subscribers.hpp * * @brief Structure for the ackermann_cmd_mux. * * License: BSD * https://raw.github.com/yujinrobot/yujin_ocs/hydro/yocs_ackermann_cmd_mux/LICENSE **/ /***************************************************************************** ** Ifdefs *****************************************************************************/ #ifndef RACECAR_ACKERMANN_CMD_SUBSCRIBERS_HPP_ #define RACECAR_ACKERMANN_CMD_SUBSCRIBERS_HPP_ /***************************************************************************** ** Includes *****************************************************************************/ #include #include #include #ifdef HAVE_NEW_YAMLCPP // The >> operator disappeared in yaml-cpp 0.5, so this function is // added to provide support for code written under the yaml-cpp 0.3 API. template void operator >> (const YAML::Node& node, T& i) { i = node.as(); } #endif /***************************************************************************** ** Preprocessing *****************************************************************************/ // move to a static const? #define VACANT std::numeric_limits::max() /***************************************************************************** ** Namespaces *****************************************************************************/ namespace ackermann_cmd_mux { /***************************************************************************** ** AckermannCmdSubscribers *****************************************************************************/ /** * Pool of ackermann_cmd topics subscribers */ class AckermannCmdSubscribers { public: /** * Inner class describing an individual subscriber to a ackermann_cmd topic */ class AckermannCmdSubs { public: unsigned int idx; /**< Index; assigned according to the order on YAML file */ std::string name; /**< Descriptive name */ ros::Subscriber subs; /**< The subscriber itself */ std::string topic; /**< The name of the topic */ ros::Timer timer; /**< No incoming messages timeout */ double timeout; /**< Timer's timeout, in seconds */ unsigned int priority; /**< UNIQUE integer from 0 (lowest priority) to MAX_INT */ std::string short_desc; /**< Short description (optional) */ bool active; /**< Whether this source is active */ AckermannCmdSubs(unsigned int idx) : idx(idx), active(false) {}; void operator << (const YAML::Node& node); }; AckermannCmdSubscribers() : allowed(VACANT) { } ~AckermannCmdSubscribers() { } std::vector::size_type size() { return list.size(); }; AckermannCmdSubs& operator [] (unsigned int idx) { return list[idx]; }; /** * @brief Configures the subscribers from a yaml file. * * @exception FileNotFoundException : yaml file not found * @exception YamlException : problem parsing the yaml * @exception EmptyCfgException : empty configuration file * @param node : node holding all the subscriber configuration */ void configure(const YAML::Node& node); unsigned int allowed; private: std::vector list; }; } // namespace ackermann_cmd_mux #endif /* ACKERMANN_CMD_SUBSCRIBERS_HPP_ */ ================================================ FILE: ackermann_cmd_mux/include/ackermann_cmd_mux/exceptions.hpp ================================================ /** * @file /ackermann_cmd_mux/include/ackermann_cmd_mux/exceptions.hpp * * @brief Exception classes for ackermann_cmd_mux. * * License: BSD * https://raw.github.com/yujinrobot/yujin_ocs/hydro/yocs_ackermann_cmd_mux/LICENSE **/ /***************************************************************************** ** Ifdefs *****************************************************************************/ #ifndef RACECAR_ACKERMANN_CMD_EXCEPTIONS_HPP_ #define RACECAR_ACKERMANN_CMD_EXCEPTIONS_HPP_ /***************************************************************************** ** Includes *****************************************************************************/ #include /***************************************************************************** ** Namespaces *****************************************************************************/ namespace ackermann_cmd_mux { /***************************************************************************** ** Exceptions *****************************************************************************/ class FileNotFoundException: public std::runtime_error { public: FileNotFoundException(const std::string& msg) : std::runtime_error(msg) {} virtual ~FileNotFoundException() throw() {} }; class EmptyCfgException: public std::runtime_error { public: EmptyCfgException() : std::runtime_error("") {} virtual ~EmptyCfgException() throw() {} }; class YamlException: public std::runtime_error { public: YamlException(const std::string& msg) : std::runtime_error(msg) {} virtual ~YamlException() throw() {} }; } // namespace ackermann_cmd_mux #endif /* RACECAR_ACKERMANN_CMD_EXCEPTIONS_HPP_ */ ================================================ FILE: ackermann_cmd_mux/launch/ackermann_cmd_mux.launch ================================================ ================================================ FILE: ackermann_cmd_mux/launch/reconfigure.launch ================================================ ================================================ FILE: ackermann_cmd_mux/launch/standalone.launch ================================================ ================================================ FILE: ackermann_cmd_mux/package.xml ================================================ ackermann_cmd_mux 0.7.0 A multiplexer for ackermann command velocity inputs. Arbitrates incoming ackermann_cmd messages from several topics, allowing one topic at a time to command the robot, based on priorities. It also deallocates current allowed topic if no messages are received after a configured timeout. All topics, together with their priority and timeout are configured through a YAML file, that can be reload at runtime. Blatantly derived / copied from yujin_ocs/yocs_cmd_vel_mux. Jorge Santos Simon Michael Boulet Michael Boulet BSD http://ros.org/wiki/ackermann_cmd_mux https://github.mit.edu/racecar/racecar https://github.mit.edu/racecar/racecar/issues catkin roscpp nodelet dynamic_reconfigure pluginlib ackermann_msgs yaml-cpp rospy std_msgs roscpp ackermann_msgs pluginlib nodelet dynamic_reconfigure yaml-cpp rospy std_msgs ================================================ FILE: ackermann_cmd_mux/param/example.yaml ================================================ # Created on: Oct 29, 2012 # Author: jorge # Configuration for subscribers to ackermann_cmd sources. This file is provided just as an example. # Typically automatic controllers, as ROS navigation stack should have the minimum priority # # Used with example.launch # # Individual subscriber configuration: # name: Source name # topic: The topic that provides ackermann_cmd messages # timeout: Time in seconds without incoming messages to consider this topic inactive # priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT # short_desc: Short description (optional) subscribers: - name: "Default input" topic: "input/default" timeout: 0.1 priority: 0 short_desc: "The default ackermann_cmd, controllers unaware that we are multiplexing ackermann_cmd should come here" - name: "Navigation stack" topic: "input/navigation" timeout: 0.5 priority: 1 short_desc: "Navigation stack controller" - name: "Onboard joystick" topic: "input/joystick" timeout: 0.1 priority: 10 - name: "Remote control" topic: "input/remote" timeout: 0.1 priority: 9 - name: "Web application" topic: "input/webapp" timeout: 0.3 priority: 8 - name: "Keyboard operation" topic: "input/keyop" timeout: 0.1 priority: 7 publisher: "output/ackermann_cmd" ================================================ FILE: ackermann_cmd_mux/param/reconfigure.yaml ================================================ # Another example configuration file, though this one is used to test the dynamic reconfiguration # of the ackermann_cmd_mux. Used with reconfigure.launch. subscribers: - name: "Default input" topic: "input/default" timeout: 0.1 priority: 0 short_desc: "The default ackermann_cmd, controllers unaware that we are multiplexing ackermann_cmd should come here" - name: "Navigation stack" topic: "input/navigation" timeout: 0.5 priority: 1 short_desc: "Navigation stack controller" - name: "Safety controller" topic: "input/safety" timeout: 0.1 priority: 10 short_desc: "Used with the reactive velocity control provided by the bumper/cliff sensor safety controller" ================================================ FILE: ackermann_cmd_mux/plugins/nodelets.xml ================================================ Implementation for the ackermann command multiplexer as a nodelet. ================================================ FILE: ackermann_cmd_mux/src/ackermann_cmd_mux_nodelet.cpp ================================================ /** * @file /src/ackermann_cmd_mux_nodelet.cpp * * @brief Implementation for the ackermann command multiplexer * * License: BSD * https://raw.github.com/yujinrobot/yujin_ocs/hydro/yocs_ackermann_cmd_mux/LICENSE **/ /***************************************************************************** ** Includes *****************************************************************************/ #include #include #include #include "ackermann_cmd_mux/ackermann_cmd_mux_nodelet.hpp" #include "ackermann_cmd_mux/exceptions.hpp" /***************************************************************************** ** Namespaces *****************************************************************************/ namespace ackermann_cmd_mux { /***************************************************************************** ** Implementation *****************************************************************************/ void AckermannCmdMuxNodelet::ackermannCmdCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& msg, unsigned int idx) { // Reset timer for this source ackermann_cmd_sub[idx].timer.stop(); ackermann_cmd_sub[idx].timer.start(); ackermann_cmd_sub[idx].active = true; // obviously his source is sending commands, so active // Give permit to publish to this source if it's the only active or is // already allowed or has higher priority that the currently allowed if ((ackermann_cmd_sub.allowed == VACANT) || (ackermann_cmd_sub.allowed == idx) || (ackermann_cmd_sub[idx].priority > ackermann_cmd_sub[ackermann_cmd_sub.allowed].priority)) { if (ackermann_cmd_sub.allowed != idx) { ackermann_cmd_sub.allowed = idx; // Notify the world that a new ackermann_cmd source took the control std_msgs::StringPtr acv_msg(new std_msgs::String); acv_msg->data = ackermann_cmd_sub[idx].name; if (active_subscriber) { active_subscriber.publish(acv_msg); } } if (mux_ackermann_cmd_pub){ mux_ackermann_cmd_pub.publish(msg); } } } void AckermannCmdMuxNodelet::timerCallback(const ros::TimerEvent& event, unsigned int idx) { if (ackermann_cmd_sub.allowed == idx) { // No ackermann_cmd messages timeout happened to currently active source, so... ackermann_cmd_sub.allowed = VACANT; // ...notify the world that nobody is publishing on ackermann_cmd; its vacant std_msgs::StringPtr acv_msg(new std_msgs::String); acv_msg->data = "idle"; if (active_subscriber){ active_subscriber.publish(acv_msg); } } ackermann_cmd_sub[idx].active = false; } void AckermannCmdMuxNodelet::onInit() { ros::NodeHandle &nh = this->getPrivateNodeHandle(); /********************* ** Dynamic Reconfigure **********************/ dynamic_reconfigure_cb = boost::bind(&AckermannCmdMuxNodelet::reloadConfiguration, this, _1, _2); dynamic_reconfigure_server = new dynamic_reconfigure::Server(nh); dynamic_reconfigure_server->setCallback(dynamic_reconfigure_cb); active_subscriber = nh.advertise ("active", 1, true); // latched topic // Notify the world that by now nobody is publishing on ackermann_cmd yet std_msgs::StringPtr active_msg(new std_msgs::String); active_msg->data = "idle"; if (active_subscriber) { active_subscriber.publish(active_msg); } // could use a call to reloadConfiguration here, but it seems to automatically call it once with defaults anyway. NODELET_DEBUG("AckermannCmdMux : successfully initialised"); } void AckermannCmdMuxNodelet::reloadConfiguration(ackermann_cmd_mux::reloadConfig &config, uint32_t unused_level) { std::string yaml_cfg_file; ros::NodeHandle &nh = this->getNodeHandle(); ros::NodeHandle &nh_priv = this->getPrivateNodeHandle(); if( config.yaml_cfg_file == "" ) { // typically fired on startup, so look for a parameter to set a default nh_priv.getParam("yaml_cfg_file", yaml_cfg_file); } else { yaml_cfg_file = config.yaml_cfg_file; } /********************* ** Yaml File Parsing **********************/ std::ifstream ifs(yaml_cfg_file.c_str(), std::ifstream::in); if (ifs.good() == false) { NODELET_ERROR_STREAM("AckermannCmdMux : configuration file not found [" << yaml_cfg_file << "]"); return; } // probably need to bring the try catches back here YAML::Node doc; #ifdef HAVE_NEW_YAMLCPP doc = YAML::Load(ifs); #else YAML::Parser parser(ifs); parser.GetNextDocument(doc); #endif /********************* ** Output Publisher **********************/ std::string output_name("output"); #ifdef HAVE_NEW_YAMLCPP if ( doc["publisher"] ) { doc["publisher"] >> output_name; } #else const YAML::Node *node = doc.FindValue("publisher"); if ( node != NULL ) { *node >> output_name; } #endif mux_ackermann_cmd_pub = nh_priv.advertise (output_name, 10); /********************* ** Input Subscribers **********************/ try { ackermann_cmd_sub.configure(doc["subscribers"]); } catch(EmptyCfgException& e) { NODELET_WARN("AckermannCmdMux : yaml configured zero subscribers, check yaml content."); } catch(YamlException& e) { NODELET_ERROR_STREAM("AckermannCmdMux : yaml parsing problem [" << std::string(e.what()) + "]"); } // Publishers and subscribers for (unsigned int i = 0; i < ackermann_cmd_sub.size(); i++) { ackermann_cmd_sub[i].subs = nh_priv.subscribe(ackermann_cmd_sub[i].topic, 10, AckermannCmdFunctor(i, this)); // Create (stopped by now) a one-shot timer for every subscriber ackermann_cmd_sub[i].timer = nh_priv.createTimer(ros::Duration(ackermann_cmd_sub[i].timeout), TimerFunctor(i, this), true, false); NODELET_DEBUG("AckermannCmdMux : subscribed to '%s' on topic '%s'. pr: %d, to: %.2f", ackermann_cmd_sub[i].name.c_str(), ackermann_cmd_sub[i].topic.c_str(), ackermann_cmd_sub[i].priority, ackermann_cmd_sub[i].timeout); } NODELET_INFO_STREAM("AckermannCmdMux : (re)configured [" << yaml_cfg_file << "]"); ifs.close(); } } // namespace ackermann_cmd_mux PLUGINLIB_EXPORT_CLASS(ackermann_cmd_mux::AckermannCmdMuxNodelet, nodelet::Nodelet); ================================================ FILE: ackermann_cmd_mux/src/ackermann_cmd_subscribers.cpp ================================================ /** * @file /src/ackermann_cmd_subscribers.cpp * * @brief Subscriber handlers for the ackermann_cmd_mux * * License: BSD * https://raw.github.com/yujinrobot/yujin_ocs/hydro/yocs_ackermann_cmd_mux/LICENSE **/ /***************************************************************************** ** Includes *****************************************************************************/ #include #include "ackermann_cmd_mux/ackermann_cmd_subscribers.hpp" #include "ackermann_cmd_mux/exceptions.hpp" /***************************************************************************** ** Namespaces *****************************************************************************/ namespace ackermann_cmd_mux { /***************************************************************************** ** Implementation *****************************************************************************/ void AckermannCmdSubscribers::AckermannCmdSubs::operator << (const YAML::Node& node) { node["name"] >> name; node["topic"] >> topic; node["timeout"] >> timeout; node["priority"] >> priority; #ifdef HAVE_NEW_YAMLCPP if (node["short_desc"]) { #else if (node.FindValue("short_desc") != NULL) { #endif node["short_desc"] >> short_desc; } } void AckermannCmdSubscribers::configure(const YAML::Node& node) { list.clear(); try { if ( node.size() == 0 ) { throw EmptyCfgException(); } for (unsigned int i = 0; i < node.size(); i++) { // Parse every entries on YAML AckermannCmdSubs subscriber(i); subscriber << node[i]; list.push_back(subscriber); } } catch(EmptyCfgException& e) { throw e; } catch(YAML::ParserException& e) { throw YamlException(e.what()); } catch(YAML::RepresentationException& e) { throw YamlException(e.what()); } } } // namespace ackermann_cmd_mux ================================================ FILE: ackermann_cmd_mux/src/throttle_interpolator.py ================================================ #!/usr/bin/env python import rospy from std_msgs.msg import Float64 # import some utils. import numpy as np import copy as copy class InterpolateThrottle: def __init__(self): # Allow our topics to be dynamic. self.rpm_input_topic = rospy.get_param('~rpm_input_topic', '/vesc/commands/motor/unsmoothed_speed') self.rpm_output_topic = rospy.get_param('~rpm_output_topic', '/vesc/commands/motor/speed') self.servo_input_topic = rospy.get_param('~servo_input_topic', '/vesc/commands/servo/unsmoothed_position') self.servo_output_topic = rospy.get_param('~servo_output_topic', '/vesc/commands/servo/position') self.max_acceleration = rospy.get_param('/vesc/max_acceleration') self.max_rpm = rospy.get_param('/vesc/vesc_driver/speed_max') self.min_rpm = rospy.get_param('/vesc/vesc_driver/speed_min') self.throttle_smoother_rate = rospy.get_param('/vesc/throttle_smoother_rate') self.speed_to_erpm_gain = rospy.get_param('/vesc/speed_to_erpm_gain') self.max_servo_speed = rospy.get_param('/vesc/max_servo_speed') self.steering_angle_to_servo_gain = rospy.get_param('/vesc/steering_angle_to_servo_gain') self.servo_smoother_rate = rospy.get_param('/vesc/servo_smoother_rate') self.max_servo = rospy.get_param('/vesc/vesc_driver/servo_max') self.min_servo = rospy.get_param('/vesc/vesc_driver/servo_min') # Variables self.last_rpm = 0 self.desired_rpm = self.last_rpm self.last_servo = rospy.get_param('/vesc/steering_angle_to_servo_offset') self.desired_servo_position = self.last_servo # Create topic subscribers and publishers self.rpm_output = rospy.Publisher(self.rpm_output_topic, Float64,queue_size=1) self.servo_output = rospy.Publisher(self.servo_output_topic, Float64,queue_size=1) rospy.Subscriber(self.rpm_input_topic, Float64, self._process_throttle_command) rospy.Subscriber(self.servo_input_topic, Float64, self._process_servo_command) self.max_delta_servo = abs(self.steering_angle_to_servo_gain * self.max_servo_speed / self.servo_smoother_rate) rospy.Timer(rospy.Duration(1.0/self.servo_smoother_rate), self._publish_servo_command) self.max_delta_rpm = abs(self.speed_to_erpm_gain * self.max_acceleration / self.throttle_smoother_rate) rospy.Timer(rospy.Duration(1.0/self.max_delta_rpm), self._publish_throttle_command) # run the node self._run() # Keep the node alive def _run(self): rospy.spin() def _publish_throttle_command(self, evt): desired_delta = self.desired_rpm-self.last_rpm clipped_delta = max(min(desired_delta, self.max_delta_rpm), -self.max_delta_rpm) smoothed_rpm = self.last_rpm + clipped_delta self.last_rpm = smoothed_rpm # print self.desired_rpm, smoothed_rpm self.rpm_output.publish(Float64(smoothed_rpm)) def _process_throttle_command(self,msg): input_rpm = msg.data # Do some sanity clipping input_rpm = min(max(input_rpm, self.min_rpm), self.max_rpm) self.desired_rpm = input_rpm def _publish_servo_command(self, evt): desired_delta = self.desired_servo_position-self.last_servo clipped_delta = max(min(desired_delta, self.max_delta_servo), -self.max_delta_servo) smoothed_servo = self.last_servo + clipped_delta self.last_servo = smoothed_servo self.servo_output.publish(Float64(smoothed_servo)) def _process_servo_command(self,msg): input_servo = msg.data # Do some sanity clipping input_servo = min(max(input_servo, self.min_servo), self.max_servo) # set the target servo position self.desired_servo_position = input_servo # Boilerplate node spin up. if __name__ == '__main__': try: rospy.init_node('Throttle_Interpolator') p = InterpolateThrottle() except rospy.ROSInterruptException: pass ================================================ FILE: racecar/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(racecar) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html # catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## ################################################ ## To declare and build messages, services or actions from within this ## package, follow these steps: ## * Let MSG_DEP_SET be the set of packages whose message types you use in ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). ## * In the file package.xml: ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET ## * If MSG_DEP_SET isn't empty the following dependencies might have been ## pulled in transitively but can be declared for certainty nonetheless: ## * add a build_depend tag for "message_generation" ## * add a run_depend tag for "message_runtime" ## * In this file (CMakeLists.txt): ## * add "message_generation" and every package in MSG_DEP_SET to ## find_package(catkin REQUIRED COMPONENTS ...) ## * add "message_runtime" and every package in MSG_DEP_SET to ## catkin_package(CATKIN_DEPENDS ...) ## * uncomment the add_*_files sections below as needed ## and list every .msg/.srv/.action file to be processed ## * uncomment the generate_messages entry below ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder # add_message_files( # FILES # Message1.msg # Message2.msg # ) ## Generate services in the 'srv' folder # add_service_files( # FILES # Service1.srv # Service2.srv # ) ## Generate actions in the 'action' folder # add_action_files( # FILES # Action1.action # Action2.action # ) ## Generate added messages and services with any dependencies listed here # generate_messages( # DEPENDENCIES # std_msgs # Or other packages containing msgs # ) ################################### ## catkin specific configuration ## ################################### ## The catkin_package macro generates cmake config files for your package ## Declare things to be passed to dependent projects ## INCLUDE_DIRS: uncomment this if you package contains header files ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include # LIBRARIES racecar_launch CATKIN_DEPENDS razor_imu_9dof tf tf2_ros urg_node joy rosbag rostopic rviz # specific to racecar-v1 #pointgrey_camera_driver #px4flow #pwm_sysfs_driver # specific to racecar-v2 # mapping gmapping hector_mapping robot_pose_ekf # DEPENDS system_lib ) ########### ## Build ## ########### ## Specify additional locations of header files ## Your package locations should be listed before other locations # include_directories(include) ## Declare a cpp library # add_library(racecar_launch # src/${PROJECT_NAME}/racecar_launch.cpp # ) ## Declare a cpp executable # add_executable(racecar_launch_node src/racecar_launch_node.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes # add_dependencies(racecar_launch_node racecar_launch_generate_messages_cpp) ## Specify libraries to link a library or executable target against # target_link_libraries(racecar_launch_node # ${catkin_LIBRARIES} # ) ############# ## Install ## ############# # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination # install(PROGRAMS # scripts/my_python_script # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark executables and/or libraries for installation # install(TARGETS racecar_launch racecar_launch_node # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark cpp header files for installation # install(DIRECTORY include/${PROJECT_NAME}/ # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} # FILES_MATCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE # ) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES # # myfile1 # # myfile2 # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) ############# ## Testing ## ############# ## Add gtest based cpp test target and link libraries # catkin_add_gtest(${PROJECT_NAME}-test test/test_racecar_launch.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() ## Add folders to be run by python nosetests # catkin_add_nosetests(test) ================================================ FILE: racecar/LICENSE ================================================ # Software License Agreement (BSD License) # # Copyright (c) 2016 Massachusetts Institute of Technology. # 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 Massachusetts Institute of Technology nor the # names of its contributors may be used to endorse or promote # products derived from this software without specific prior # written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. ================================================ FILE: racecar/config/racecar-v2/high_level_mux.yaml ================================================ # Individual subscriber configuration: # name: Source name # topic: The topic that provides ackermann_cmd messages # timeout: Time in seconds without incoming messages to consider this topic inactive # priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT # short_desc: Short description (optional) subscribers: - name: "Nav_0" topic: "input/nav_0" timeout: 0.2 priority: 20 short_desc: "Input for autonomous navigation, highest priority" - name: "Nav_1" topic: "input/nav_1" timeout: 0.2 priority: 15 short_desc: "Input for autonomous navigation" - name: "Nav_2" topic: "input/nav_2" timeout: 0.2 priority: 10 short_desc: "Input for autonomous navigation" - name: "Nav_3" topic: "input/nav_3" timeout: 0.2 priority: 5 short_desc: "Input for autonomous navigation" - name: "Default" topic: "input/default" timeout: 1.0 priority: 0 short_desc: "Default input, lowest priority" publisher: "output" ================================================ FILE: racecar/config/racecar-v2/joy_teleop.yaml ================================================ joy_node: deadzone: 0.01 autorepeat_rate: 20 coalesce_interval: 0.01 teleop: # Default mode - Stop for safety default: type: topic is_default: true message_type: ackermann_msgs/AckermannDriveStamped topic_name: low_level/ackermann_cmd_mux/input/teleop message_value: - target: drive.speed value: 0.0 - target: drive.steering_angle value: 0.0 # Enable Human control by holding Left Bumper human_control: type: topic message_type: ackermann_msgs/AckermannDriveStamped topic_name: low_level/ackermann_cmd_mux/input/teleop deadman_buttons: [4] axis_mappings: - axis: 1 target: drive.speed scale: 2.0 # joystick will command plus or minus 2 meters / second offset: 0.0 - axis: 3 target: drive.steering_angle scale: 0.34 # joystick will command plus or minus ~20 degrees steering angle offset: 0.0 # Enable autonomous control by pressing right bumper # This switch causes the joy_teleop to stop sending messages to input/teleop # And send messages to /dev/null (an unused ROS topic) autonomous_control: type: topic message_type: std_msgs/Int8 topic_name: /dev/null deadman_buttons: [5] message_value: - target: data value: 0 ================================================ FILE: racecar/config/racecar-v2/low_level_mux.yaml ================================================ # Individual subscriber configuration: # name: Source name # topic: The topic that provides ackermann_cmd messages # timeout: Time in seconds without incoming messages to consider this topic inactive # priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT # short_desc: Short description (optional) subscribers: - name: "Teleoperation" topic: "input/teleop" timeout: 0.2 priority: 10 short_desc: "Input for human teleoperation (joystick). Highest priority." - name: "Safety" topic: "input/safety" timeout: 0.2 priority: 5 short_desc: "Input for safety monitor." - name: "Navigation" topic: "input/navigation" timeout: 0.2 priority: 0 short_desc: "Input for autonomous navigation" publisher: "output" ================================================ FILE: racecar/config/racecar-v2/sensors.yaml ================================================ laser_node: ip_address: 192.168.0.10 imu_node: device: /dev/imu port: /dev/ttyIMU ================================================ FILE: racecar/config/racecar-v2/vesc.yaml ================================================ # erpm (electrical rpm) = speed_to_erpm_gain * speed (meters / second) + speed_to_erpm_offset # for offset=0. speed_to_erpm_gain = num_motor_poles*60/circumference_wheel_in_meters speed_to_erpm_gain: 4614 speed_to_erpm_offset: 0.0 tachometer_ticks_to_meters_gain: 0.00225 # servo smoother - limits rotation speed and smooths anything above limit max_servo_speed: 3.2 # radians/second servo_smoother_rate: 75.0 # messages/sec # servo smoother - limits acceleration and smooths anything above limit max_acceleration: 3.5 # meters/second^2 throttle_smoother_rate: 75.0 # messages/sec # servo value (0 to 1) = steering_angle_to_servo_gain * steering angle (radians) + steering_angle_to_servo_offset steering_angle_to_servo_gain: -1.2135 steering_angle_to_servo_offset: 0.5304 # publish odom to base link tf vesc_to_odom/publish_tf: false # car wheelbase is about 25cm wheelbase: .25 vesc_driver: port: /dev/ttyVESC duty_cycle_min: 0.0 duty_cycle_max: 0.0 current_min: 0.0 current_max: 20.0 brake_min: -20000.0 brake_max: 200000.0 speed_min: -3250 speed_max: 10000 position_min: 0.0 position_max: 0.0 servo_min: 0.15 servo_max: 0.85 ================================================ FILE: racecar/launch/includes/common/joy_teleop.launch.xml ================================================ ================================================ FILE: racecar/launch/includes/common/sensors.launch.xml ================================================ ================================================ FILE: racecar/launch/includes/racecar-v2/static_transforms.launch.xml ================================================ ================================================ FILE: racecar/launch/includes/racecar-v2/velodyne.launch.xml ================================================ ================================================ FILE: racecar/launch/includes/racecar-v2/vesc.launch.xml ================================================ ================================================ FILE: racecar/launch/includes/racecar-v2-teleop.launch.xml ================================================ ================================================ FILE: racecar/launch/known_map_localization.launch ================================================ ================================================ FILE: racecar/launch/mux.launch ================================================ ================================================ FILE: racecar/launch/replay_bag_file/replay_bag_file.launch ================================================ ================================================ FILE: racecar/launch/replay_bag_file/replay_bag_mapping.launch ================================================ ================================================ FILE: racecar/launch/replay_bag_file/replay_bag_with_lidar_processing.launch ================================================ ================================================ FILE: racecar/launch/teleop.launch ================================================ ================================================ FILE: racecar/maps/basement_hallways_10cm.yaml ================================================ image: basement_hallways_10cm.png resolution: 0.100000 origin: [-30.000000, -30.000000, 0.000000] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196 ================================================ FILE: racecar/maps/basement_hallways_5cm.yml ================================================ image: basement_hallways_5cm.png resolution: 0.050000 origin: [-30.000000, -30.000000, 0.000000] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196 ================================================ FILE: racecar/maps/short-course-33.yml ================================================ image: short-course-33.png resolution: 0.025000 origin: [-11.50000, -5.000000, -0.090000] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196 ================================================ FILE: racecar/package.xml ================================================ racecar 0.0.1 RACECAR launch scripts Michael Boulet Michael Boulet BSD catkin razor_imu_9dof tf tf2_ros urg_node joy rosbag rostopic rviz gmapping hector_mapping robot_pose_ekf ================================================ FILE: racecar/rviz/known_map_localization.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /TF1/Tree1/map1 - /TF1/Tree1/map1/odom1 - /TF1/Tree1/map1/odom1/base_footprint1 - /TF1/Tree1/map1/odom1/base_footprint1/base_link1 Splitter Ratio: 0.5 Tree Height: 531 - 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: 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: 10 Reference Frame: map Value: true - Class: rviz/TF Enabled: false Frame Timeout: 15 Frames: All Enabled: false base_footprint: Value: true base_link: Value: false camera_depth_frame: Value: false camera_depth_optical_frame: Value: false camera_link: Value: false camera_rgb_frame: Value: false camera_rgb_optical_frame: Value: false caster_back_link: Value: false caster_front_link: Value: false cliff_sensor_front_link: Value: false cliff_sensor_left_link: Value: false cliff_sensor_right_link: Value: false gyro_link: Value: false map: Value: true odom: Value: true plate_bottom_link: Value: false plate_middle_link: Value: false plate_top_link: Value: false pole_bottom_0_link: Value: false pole_bottom_1_link: Value: false pole_bottom_2_link: Value: false pole_bottom_3_link: Value: false pole_bottom_4_link: Value: false pole_bottom_5_link: Value: false pole_kinect_0_link: Value: false pole_kinect_1_link: Value: false pole_middle_0_link: Value: false pole_middle_1_link: Value: false pole_middle_2_link: Value: false pole_middle_3_link: Value: false pole_top_0_link: Value: false pole_top_1_link: Value: false pole_top_2_link: Value: false pole_top_3_link: Value: false wheel_left_link: Value: false wheel_right_link: Value: false Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: map: odom: base_footprint: base_link: camera_rgb_frame: camera_depth_frame: camera_depth_optical_frame: {} camera_link: {} camera_rgb_optical_frame: {} caster_back_link: {} caster_front_link: {} cliff_sensor_front_link: {} cliff_sensor_left_link: {} cliff_sensor_right_link: {} gyro_link: {} plate_bottom_link: {} plate_middle_link: {} plate_top_link: {} pole_bottom_0_link: {} pole_bottom_1_link: {} pole_bottom_2_link: {} pole_bottom_3_link: {} pole_bottom_4_link: {} pole_bottom_5_link: {} pole_kinect_0_link: {} pole_kinect_1_link: {} pole_middle_0_link: {} pole_middle_1_link: {} pole_middle_2_link: {} pole_middle_3_link: {} pole_top_0_link: {} pole_top_1_link: {} pole_top_2_link: {} pole_top_3_link: {} wheel_left_link: {} wheel_right_link: {} Update Interval: 0 Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 255; 0; 0 Color Transformer: FlatColor 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: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.03 Style: Points Topic: /scan Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 0.7 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: /map Value: true - Arrow Length: 0.2 Class: rviz/PoseArray Color: 0; 192; 0 Enabled: true Name: Amcl Particles Topic: /particlecloud Value: true - Angle Tolerance: 0.1 Class: rviz/Odometry Color: 0; 255; 255 Enabled: true Keep: 500 Length: 0.1 Name: Odometry Position Tolerance: 0.1 Topic: /vesc/odom Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Fixed Frame: map 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/Measure Value: true Views: Current: Angle: -1.935 Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Name: Current View Near Clip Distance: 0.01 Scale: 100.79 Target Frame: base_footprint Value: TopDownOrtho (rviz) X: 1.71641 Y: 0.715559 Saved: ~ Window Geometry: Displays: collapsed: false Height: 744 Hide Left Dock: false Hide Right Dock: false QMainWindow State: 000000ff00000000fd000000040000000000000142000002a2fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002a2000000dd00ffffff000000010000010f000002a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002a2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002b8000002a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1301 X: 57 Y: -4 ================================================ FILE: racecar/rviz/laser_scan_matcher.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /Grid1 - /LaserScan1 - /TF1 - /TF1/Frames1 Splitter Ratio: 0.5 Tree Height: 434 - 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: 10 Reference Frame: Value: true - Alpha: 0.2 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/LaserScan Color: 255; 0; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 0; 0 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.025 Style: Flat Squares Topic: /scan Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true base_link: Value: true laser: Value: true world: Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: world: base_link: laser: {} Update Interval: 0 Value: true Enabled: true Global Options: Background Color: 0; 0; 0 Fixed Frame: world Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Angle: 0 Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Name: Current View Near Clip Distance: 0.01 Scale: 150 Target Frame: Value: TopDownOrtho (rviz) X: 0 Y: 0 Saved: ~ Window Geometry: Displays: collapsed: false Height: 721 Hide Left Dock: false Hide Right Dock: false QMainWindow State: 000000ff00000000fd00000004000000000000013c0000023dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000360000023d000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000023dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000360000023d0000009b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004000000003efc0100000002fb0000000800540069006d00650100000000000004000000024500fffffffb0000000800540069006d00650100000000000004500000000000000000000001a90000023d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1024 X: -2 Y: -2 ================================================ FILE: racecar/rviz/mapping.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /Map1 - /LaserScan1 Splitter Ratio: 0.5 Tree Height: 565 - 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: 10 Reference Frame: Value: true - Alpha: 0.7 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: /map Value: true - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true base_footprint: Value: true base_imu_link: Value: true base_link: Value: true camera: Value: true gmapping_map: Value: true hector_map: Value: true hector_odom: Value: true hector_scanmatcher_frame: Value: true laser: Value: true px4flow: Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: gmapping_map: hector_map: hector_odom: base_link: base_footprint: {} base_imu_link: {} camera: {} laser: {} px4flow: {} hector_scanmatcher_frame: {} Update Interval: 0 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/LaserScan Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 1030 Min Color: 0; 0; 0 Min Intensity: 370 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.08 Style: Flat Squares Topic: /scan Use Fixed Frame: true Use rainbow: true Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Fixed Frame: gmapping_map Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Topic: /initialpose - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Angle: 0 Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Name: Current View Near Clip Distance: 0.01 Scale: 8.5022 Target Frame: Value: TopDownOrtho (rviz) X: -0.8955 Y: -15.9022 Saved: ~ Window Geometry: Displays: collapsed: false Height: 846 Hide Left Dock: false Hide Right Dock: false QMainWindow State: 000000ff00000000fd00000004000000000000013c000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1200 X: 372 Y: 50 ================================================ FILE: racecar/scripts/joy_teleop.py ================================================ #!/usr/bin/env python import importlib import rospy import genpy.message from rospy import ROSException import sensor_msgs.msg import actionlib import rostopic import rosservice from threading import Thread from rosservice import ROSServiceException import numpy as np class JoyTeleopException(Exception): pass ''' Originally from https://github.com/ros-teleop/teleop_tools Pulled on April 28, 2017. Edited by Winter Guerra on April 28, 2017 to allow for default actions. ''' class JoyTeleop: """ Generic joystick teleoperation node. Will not start without configuration, has to be stored in 'teleop' parameter. See config/joy_teleop.yaml for an example. """ def __init__(self): if not rospy.has_param("teleop"): rospy.logfatal("no configuration was found, taking node down") raise JoyTeleopException("no config") self.publishers = {} self.al_clients = {} self.srv_clients = {} self.service_types = {} self.message_types = {} self.command_list = {} self.offline_actions = [] self.offline_services = [] self.old_buttons = [] teleop_cfg = rospy.get_param("teleop") for i in teleop_cfg: if i in self.command_list: rospy.logerr("command {} was duplicated".format(i)) continue action_type = teleop_cfg[i]['type'] self.add_command(i, teleop_cfg[i]) if action_type == 'topic': self.register_topic(i, teleop_cfg[i]) elif action_type == 'action': self.register_action(i, teleop_cfg[i]) elif action_type == 'service': self.register_service(i, teleop_cfg[i]) else: rospy.logerr("unknown type '%s' for command '%s'", action_type, i) # Don't subscribe until everything has been initialized. rospy.Subscriber('joy', sensor_msgs.msg.Joy, self.joy_callback) # Run a low-freq action updater rospy.Timer(rospy.Duration(2.0), self.update_actions) def joy_callback(self, data): try: for c in self.command_list: if self.match_command(c, data.buttons): self.run_command(c, data) # Only run 1 command at a time break except JoyTeleopException as e: rospy.logerr("error while parsing joystick input: %s", str(e)) self.old_buttons = data.buttons def register_topic(self, name, command): """Add a topic publisher for a joystick command""" topic_name = command['topic_name'] try: topic_type = self.get_message_type(command['message_type']) self.publishers[topic_name] = rospy.Publisher(topic_name, topic_type, queue_size=1) except JoyTeleopException as e: rospy.logerr("could not register topic for command {}: {}".format(name, str(e))) def register_action(self, name, command): """Add an action client for a joystick command""" action_name = command['action_name'] try: action_type = self.get_message_type(self.get_action_type(action_name)) self.al_clients[action_name] = actionlib.SimpleActionClient(action_name, action_type) if action_name in self.offline_actions: self.offline_actions.remove(action_name) except JoyTeleopException: if action_name not in self.offline_actions: self.offline_actions.append(action_name) class AsyncServiceProxy(object): def __init__(self, name, service_class, persistent=True): try: rospy.wait_for_service(name, timeout=2.0) except ROSException: raise JoyTeleopException("Service {} is not available".format(name)) self._service_proxy = rospy.ServiceProxy(name, service_class, persistent) self._thread = Thread(target=self._service_proxy, args=[]) def __del__(self): # try to join our thread - no way I know of to interrupt a service # request if self._thread.is_alive(): self._thread.join(1.0) def __call__(self, request): if self._thread.is_alive(): self._thread.join(0.01) if self._thread.is_alive(): return False self._thread = Thread(target=self._service_proxy, args=[request]) self._thread.start() return True def register_service(self, name, command): """ Add an AsyncServiceProxy for a joystick command """ service_name = command['service_name'] try: service_type = self.get_service_type(service_name) self.srv_clients[service_name] = self.AsyncServiceProxy( service_name, service_type) if service_name in self.offline_services: self.offline_services.remove(service_name) except JoyTeleopException: if service_name not in self.offline_services: self.offline_services.append(service_name) def match_command(self, c, buttons): """Find a command matching a joystick configuration""" # Buttons is a vector of the shape [0,1,0,1.... # Turn it into a vector of form [1, 3... button_indexes = np.argwhere(buttons).flatten() # Check if the pressed buttons match the commands exactly. buttons_match = np.array_equal(self.command_list[c]['buttons'], button_indexes) #print button_indexes if buttons_match: return True # This might also be a default command. # We need to check if ANY commands match this set of pressed buttons. any_commands_matched = np.any([ np.array_equal(command['buttons'], button_indexes) for name, command in self.command_list.iteritems()]) # Return the final result. return (buttons_match) or (not any_commands_matched and self.command_list[c]['is_default']) def add_command(self, name, command): """Add a command to the command list""" # Check if this is a default command if 'is_default' not in command: command['is_default'] = False if command['type'] == 'topic': if 'deadman_buttons' not in command: command['deadman_buttons'] = [] command['buttons'] = command['deadman_buttons'] elif command['type'] == 'action': if 'action_goal' not in command: command['action_goal'] = {} elif command['type'] == 'service': if 'service_request' not in command: command['service_request'] = {} self.command_list[name] = command def run_command(self, command, joy_state): """Run a joystick command""" cmd = self.command_list[command] if cmd['type'] == 'topic': self.run_topic(command, joy_state) elif cmd['type'] == 'action': if cmd['action_name'] in self.offline_actions: rospy.logerr("command {} was not played because the action " "server was unavailable. Trying to reconnect..." .format(cmd['action_name'])) self.register_action(command, self.command_list[command]) else: if joy_state.buttons != self.old_buttons: self.run_action(command, joy_state) elif cmd['type'] == 'service': if cmd['service_name'] in self.offline_services: rospy.logerr("command {} was not played because the service " "server was unavailable. Trying to reconnect..." .format(cmd['service_name'])) self.register_service(command, self.command_list[command]) else: if joy_state.buttons != self.old_buttons: self.run_service(command, joy_state) else: raise JoyTeleopException('command {} is neither a topic publisher nor an action or service client' .format(command)) def run_topic(self, c, joy_state): cmd = self.command_list[c] msg = self.get_message_type(cmd['message_type'])() if 'message_value' in cmd: for param in cmd['message_value']: self.set_member(msg, param['target'], param['value']) else: for mapping in cmd['axis_mappings']: if len(joy_state.axes)<=mapping['axis']: rospy.logerr('Joystick has only {} axes (indexed from 0), but #{} was referenced in config.'.format(len(joy_state.axes), mapping['axis'])) val = 0.0 else: val = joy_state.axes[mapping['axis']] * mapping.get('scale', 1.0) + mapping.get('offset', 0.0) self.set_member(msg, mapping['target'], val) self.publishers[cmd['topic_name']].publish(msg) def run_action(self, c, joy_state): cmd = self.command_list[c] goal = self.get_message_type(self.get_action_type(cmd['action_name'])[:-6] + 'Goal')() genpy.message.fill_message_args(goal, [cmd['action_goal']]) self.al_clients[cmd['action_name']].send_goal(goal) def run_service(self, c, joy_state): cmd = self.command_list[c] request = self.get_service_type(cmd['service_name'])._request_class() # should work for requests, too genpy.message.fill_message_args(request, [cmd['service_request']]) if not self.srv_clients[cmd['service_name']](request): rospy.loginfo('Not sending new service request for command {} because previous request has not finished' .format(c)) def set_member(self, msg, member, value): ml = member.split('.') if len(ml) < 1: return target = msg for i in ml[:-1]: target = getattr(target, i) setattr(target, ml[-1], value) def get_message_type(self, type_name): if type_name not in self.message_types: try: package, message = type_name.split('/') mod = importlib.import_module(package + '.msg') self.message_types[type_name] = getattr(mod, message) except ValueError: raise JoyTeleopException("message type format error") except ImportError: raise JoyTeleopException("module {} could not be loaded".format(package)) except AttributeError: raise JoyTeleopException("message {} could not be loaded from module {}".format(package, message)) return self.message_types[type_name] def get_action_type(self, action_name): try: return rostopic._get_topic_type(rospy.resolve_name(action_name) + '/goal')[0][:-4] except TypeError: raise JoyTeleopException("could not find action {}".format(action_name)) def get_service_type(self, service_name): if service_name not in self.service_types: try: self.service_types[service_name] = rosservice.get_service_class_by_name(service_name) except ROSServiceException, e: raise JoyTeleopException("service {} could not be loaded: {}".format(service_name, str(e))) return self.service_types[service_name] def update_actions(self, evt=None): for name, cmd in self.command_list.iteritems(): if cmd['type'] != 'action': continue if cmd['action_name'] in self.offline_actions: self.register_action(name, cmd) if __name__ == "__main__": try: rospy.init_node('joy_teleop') jt = JoyTeleop() rospy.spin() except JoyTeleopException: pass except rospy.ROSInterruptException: pass ================================================ FILE: racecar-vm.rosinstall ================================================ - setup-file: {local-name: /opt/ros/kinetic/setup.sh} - git: {local-name: src/racecar, version: master, uri: 'https://github.com/mit-racecar/racecar.git'} - git: {local-name: src/vesc, version: master, uri: 'https://github.com/mit-racecar/vesc.git'} - git: {local-name: src/racecar-simulator, version: master, uri: 'https://github.com/mit-racecar/racecar-simulator.git'} ================================================ FILE: racecar.rosinstall ================================================ - setup-file: {local-name: /opt/ros/kinetic/setup.sh} - git: {local-name: src/racecar, version: master, uri: 'https://github.com/mit-racecar/racecar.git'} - git: {local-name: src/vesc, version: master, uri: 'https://github.com/mit-racecar/vesc.git'} - git: {local-name: src/zed_wrapper, version: 6c06bb0a3c32aa2dec4a539e296f6ee1d6937518, uri: 'https://github.com/stereolabs/zed-ros-wrapper.git'}