exercise5 stop/start finished

exercise5/ex5.launch

@@ -0,0 +1,15 @@
+<launch>
+<node pkg="husky_highlevel_controller" type="husky_highlevel_controller" 
+    name="husky_highlevel_controller" output="screen">
+    <rosparam command="load"
+        file="$(find husky_highlevel_controller)/config/config.yaml"/>
+</node>
+
+<node pkg="rviz" type="rviz" name="rviz">
+</node>
+
+<include file="$(find husky_gazebo)/launch/husky_empty_world.launch">
+    <arg name="world_name" value="$(find husky_highlevel_controller)/worlds/singlePillar.world"/>
+    <arg name="laser_enabled" value="true"/>
+</include>
+</launch>

exercise5/husky_highlevel_controller/include/husky_highlevel_controller/HuskyHighlevelController.hpp

@@ -32,6 +32,9 @@ class HuskyHighlevelController {
 
 	ros::ServiceServer service_;
 
+	// variables
+	bool pause;
+
 	// parameters
   float p_gain;
   float x_dot;

exercise5/husky_highlevel_controller/src/HuskyHighlevelController.cpp

@@ -8,6 +8,9 @@ HuskyHighlevelController::HuskyHighlevelController(ros::NodeHandle& nodeHandle)
   std::string topic;
   int queue_size;
 
+  // initialize variables
+  pause = false;
+
   // initialize parameters
   p_gain = 0;
   x_dot = 0;
@@ -78,8 +81,18 @@ void HuskyHighlevelController::topicCallback(const sensor_msgs::LaserScan::Const
   // calculate control input
   float angle_dot, x_dot;
 
-  angle_dot = HuskyHighlevelController::p_gain * (0 - rad);
-  x_dot = HuskyHighlevelController::x_dot;
+  angle_dot = 0;
+  x_dot = 0;
+
+  if (pause == true) {
+    // pause
+    angle_dot = 0;
+    x_dot = 0;
+  } else {
+    // resume
+    angle_dot = HuskyHighlevelController::p_gain * (0 - rad);
+    x_dot = HuskyHighlevelController::x_dot;
+  }
 
   // publish control input as message
   geometry_msgs::Twist base_cmd;
@@ -119,7 +132,19 @@ void HuskyHighlevelController::topicCallback(const sensor_msgs::LaserScan::Const
 bool HuskyHighlevelController::serviceCallback(std_srvs::SetBool::Request& request,
                                                std_srvs::SetBool::Response& response)
 {
-  ROS_INFO("pause husky");
+  bool input = request.data;
+
+  if (input == true) {
+    pause = true;
+    response.message = "pause husky";
+    ROS_INFO("pause husky");
+  } else {
+    pause = false;
+    response.message = "resume husky";
+    ROS_INFO("resume husky");
+  }
+
+  response.success = true;
   return true;
 }
 

exercise5/ros_package_template/CMakeLists.txt

@@ -0,0 +1,103 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ros_package_template)
+
+## Use C++11
+add_definitions(--std=c++11)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED
+  COMPONENTS
+    roscpp
+    sensor_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
+  CATKIN_DEPENDS
+    sensor_msgs
+#  DEPENDS
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+add_library(${PROJECT_NAME}_core
+   src/Algorithm.cpp
+)
+
+## Declare cpp executables
+add_executable(${PROJECT_NAME}
+  src/${PROJECT_NAME}_node.cpp
+  src/RosPackageTemplate.cpp
+)
+
+## Specify libraries to link executable targets against
+target_link_libraries(${PROJECT_NAME}_core
+  ${catkin_LIBRARIES}
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${PROJECT_NAME}_core
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# Mark executables and/or libraries for installation
+install(
+  TARGETS ${PROJECT_NAME}
+  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 "*.hpp"
+)
+
+# Mark other files for installation
+install(
+  DIRECTORY doc
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+if(CATKIN_ENABLE_TESTING)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+## Add gtest based cpp test target and link libraries
+catkin_add_gtest(${PROJECT_NAME}-test
+  test/test_ros_package_template.cpp
+  test/AlgorithmTest.cpp)
+endif()
+
+if(TARGET ${PROJECT_NAME}-test)
+  target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core)
+endif()

exercise5/ros_package_template/LICENSE

@@ -0,0 +1,29 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2017, Peter Fankhauser
+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 Autonomous Systems Lab nor ETH Zurich nor the names
+  of its contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

exercise5/ros_package_template/README.md

@@ -0,0 +1,130 @@
+# Package Name
+
+## Overview
+
+This is a template: replace, remove, and add where required. Describe here what this package does and what it's meant for in a few sentences.
+
+The PACKAGE NAME package has been tested under [ROS] Indigo and Ubuntu 14.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed.
+
+The source code is released under a [BSD 3-Clause license](ros_package_template/LICENSE).
+
+**Author(s): Péter Fankhauser   
+Maintainer: Péter Fankhauser, [email protected]  
+Affiliation: Autonomous Systems Lab, ETH Zurich**
+
+![Example image](doc/example.jpg)
+
+
+### Publications
+
+If you use this work in an academic context, please cite the following publication(s):
+
+* P. Fankhauser, M. Bloesch, C. Gehring, M. Hutter, and R. Siegwart: **PAPER TITLE**. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2015. ([PDF](http://dx.doi.org/10.3929/ethz-a-010173654))
+
+        @inproceedings{Fankhauser2015,
+            author = {Fankhauser, P\'{e}ter and Hutter, Marco},
+            booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
+            title = {{PAPER TITLE}},
+            publisher = {IEEE},
+            year = {2015}
+        }
+
+
+## Installation
+
+### Installation from Packages
+
+To install all packages from the this repository as Debian packages use
+
+    sudo apt-get install ros-indigo-...
+
+### Building from Source
+
+#### Dependencies
+
+- [Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics),
+- [Eigen] (linear algebra library)
+
+		sudo apt-get install libeigen3-dev
+
+
+#### Building
+
+To build from source, clone the latest version from this repository into your catkin workspace and compile the package using
+
+	cd catkin_workspace/src
+	git clone https://github.com/ethz-asl/ros_package_template.git
+	cd ../
+	catkin_make
+
+
+### Unit Tests
+
+Run the unit tests with
+
+	catkin_make run_tests_ros_package_template run_tests_ros_package_template
+
+
+## Usage
+
+Describe the quickest way to run this software, for example:
+
+Run the main node with
+
+	roslaunch ros_package_template ros_package_template.launch
+
+
+## Nodes
+
+### ros_package_template
+
+Reads temperature measurements and computed the average.
+
+
+#### Subscribed Topics
+
+* **`/temperature`** ([sensor_msgs/Temperature])
+
+	The temperature measurements from which the average is computed.
+
+
+#### Published Topics
+
+...
+
+
+#### Services
+
+* **`get_average`** ([std_srvs/Trigger])
+
+	Returns information about the current average. For example, you can trigger the computation from the console with
+
+		rosservice call /ros_package_template/get_average
+
+
+#### Parameters
+
+* **`subscriber_topic`** (string, default: "/temperature")
+
+	The name of the input topic.
+
+* **`cache_size`** (int, default: 200, min: 0, max: 1000)
+
+	The size of the cache.
+
+
+### NODE_B_NAME
+
+...
+
+
+## Bugs & Feature Requests
+
+Please report bugs and request features using the [Issue Tracker](https://github.com/ethz-asl/ros_best_practices/issues).
+
+
+[ROS]: http://www.ros.org
+[rviz]: http://wiki.ros.org/rviz
+[Eigen]: http://eigen.tuxfamily.org
+[std_srvs/Trigger]: http://docs.ros.org/api/std_srvs/html/srv/Trigger.html
+[sensor_msgs/Temperature]: http://docs.ros.org/api/sensor_msgs/html/msg/Temperature.html

exercise5/ros_package_template/config/default.yaml

@@ -0,0 +1 @@
+subscriber_topic: /temperature

exercise5/ros_package_template/include/ros_package_template/Algorithm.hpp

@@ -0,0 +1,42 @@
+#pragma once
+
+namespace ros_package_template {
+
+/*!
+ * Class containing the algorithmic part of the package.
+ */
+class Algorithm
+{
+ public:
+  /*!
+   * Constructor.
+   */
+  Algorithm();
+
+  /*!
+   * Destructor.
+   */
+  virtual ~Algorithm();
+
+  /*!
+   * Add new measurement data.
+   * @param data the new data.
+   */
+  void addData(const double data);
+
+  /*!
+   * Get the computed average of the data.
+   * @return the average of the data.
+   */
+  double getAverage() const;
+
+ private:
+
+  //! Internal variable to hold the current average.
+  double average_;
+
+  //! Number of measurements taken.
+  unsigned int nMeasurements_;
+};
+
+} /* namespace */