Skip to content

Commit aaf0418

Browse files
eastskykangeastskykang
committed
exercise5 stop/start finished
1 parent 4951fad commit aaf0418

File tree

17 files changed

+579
-3
lines changed

17 files changed

+579
-3
lines changed

‎exercise5/ex5.launch

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
<launch>
2+
<node pkg="husky_highlevel_controller" type="husky_highlevel_controller"
3+
name="husky_highlevel_controller" output="screen">
4+
<rosparam command="load"
5+
file="$(find husky_highlevel_controller)/config/config.yaml"/>
6+
</node>
7+
8+
<node pkg="rviz" type="rviz" name="rviz">
9+
</node>
10+
11+
<include file="$(find husky_gazebo)/launch/husky_empty_world.launch">
12+
<arg name="world_name" value="$(find husky_highlevel_controller)/worlds/singlePillar.world"/>
13+
<arg name="laser_enabled" value="true"/>
14+
</include>
15+
</launch>

‎exercise5/husky_highlevel_controller/include/husky_highlevel_controller/HuskyHighlevelController.hpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,9 @@ class HuskyHighlevelController {
3232

3333
ros::ServiceServer service_;
3434

35+
// variables
36+
bool pause;
37+
3538
// parameters
3639
float p_gain;
3740
float x_dot;

‎exercise5/husky_highlevel_controller/src/HuskyHighlevelController.cpp

Lines changed: 28 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,9 @@ HuskyHighlevelController::HuskyHighlevelController(ros::NodeHandle& nodeHandle)
88
std::string topic;
99
int queue_size;
1010

11+
// initialize variables
12+
pause = false;
13+
1114
// initialize parameters
1215
p_gain = 0;
1316
x_dot = 0;
@@ -78,8 +81,18 @@ void HuskyHighlevelController::topicCallback(const sensor_msgs::LaserScan::Const
7881
// calculate control input
7982
float angle_dot, x_dot;
8083

81-
angle_dot = HuskyHighlevelController::p_gain * (0 - rad);
82-
x_dot = HuskyHighlevelController::x_dot;
84+
angle_dot = 0;
85+
x_dot = 0;
86+
87+
if (pause == true) {
88+
// pause
89+
angle_dot = 0;
90+
x_dot = 0;
91+
} else {
92+
// resume
93+
angle_dot = HuskyHighlevelController::p_gain * (0 - rad);
94+
x_dot = HuskyHighlevelController::x_dot;
95+
}
8396

8497
// publish control input as message
8598
geometry_msgs::Twist base_cmd;
@@ -119,7 +132,19 @@ void HuskyHighlevelController::topicCallback(const sensor_msgs::LaserScan::Const
119132
bool HuskyHighlevelController::serviceCallback(std_srvs::SetBool::Request& request,
120133
std_srvs::SetBool::Response& response)
121134
{
122-
ROS_INFO("pause husky");
135+
bool input = request.data;
136+
137+
if (input == true) {
138+
pause = true;
139+
response.message = "pause husky";
140+
ROS_INFO("pause husky");
141+
} else {
142+
pause = false;
143+
response.message = "resume husky";
144+
ROS_INFO("resume husky");
145+
}
146+
147+
response.success = true;
123148
return true;
124149
}
125150

‎exercise5/ros_package_template/CMakeLists.txt

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
cmake_minimum_required(VERSION 2.8.3)
2+
project(ros_package_template)
3+
4+
## Use C++11
5+
add_definitions(--std=c++11)
6+
7+
## Find catkin macros and libraries
8+
find_package(catkin REQUIRED
9+
COMPONENTS
10+
roscpp
11+
sensor_msgs
12+
)
13+
14+
###################################
15+
## catkin specific configuration ##
16+
###################################
17+
## The catkin_package macro generates cmake config files for your package
18+
## Declare things to be passed to dependent projects
19+
## INCLUDE_DIRS: uncomment this if you package contains header files
20+
## LIBRARIES: libraries you create in this project that dependent projects also need
21+
## CATKIN_DEPENDS: catkin_packages dependent projects also need
22+
## DEPENDS: system dependencies of this project that dependent projects also need
23+
catkin_package(
24+
INCLUDE_DIRS
25+
include
26+
# LIBRARIES
27+
CATKIN_DEPENDS
28+
sensor_msgs
29+
# DEPENDS
30+
)
31+
32+
###########
33+
## Build ##
34+
###########
35+
36+
## Specify additional locations of header files
37+
## Your package locations should be listed before other locations
38+
include_directories(
39+
include
40+
${catkin_INCLUDE_DIRS}
41+
)
42+
43+
## Declare a cpp library
44+
add_library(${PROJECT_NAME}_core
45+
src/Algorithm.cpp
46+
)
47+
48+
## Declare cpp executables
49+
add_executable(${PROJECT_NAME}
50+
src/${PROJECT_NAME}_node.cpp
51+
src/RosPackageTemplate.cpp
52+
)
53+
54+
## Specify libraries to link executable targets against
55+
target_link_libraries(${PROJECT_NAME}_core
56+
${catkin_LIBRARIES}
57+
)
58+
59+
target_link_libraries(${PROJECT_NAME}
60+
${PROJECT_NAME}_core
61+
${catkin_LIBRARIES}
62+
)
63+
64+
#############
65+
## Install ##
66+
#############
67+
68+
# Mark executables and/or libraries for installation
69+
install(
70+
TARGETS ${PROJECT_NAME}
71+
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
72+
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
73+
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
74+
)
75+
76+
# Mark cpp header files for installation
77+
install(
78+
DIRECTORY include/${PROJECT_NAME}/
79+
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
80+
FILES_MATCHING PATTERN "*.hpp"
81+
)
82+
83+
# Mark other files for installation
84+
install(
85+
DIRECTORY doc
86+
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
87+
)
88+
89+
#############
90+
## Testing ##
91+
#############
92+
93+
if(CATKIN_ENABLE_TESTING)
94+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
95+
## Add gtest based cpp test target and link libraries
96+
catkin_add_gtest(${PROJECT_NAME}-test
97+
test/test_ros_package_template.cpp
98+
test/AlgorithmTest.cpp)
99+
endif()
100+
101+
if(TARGET ${PROJECT_NAME}-test)
102+
target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core)
103+
endif()

‎exercise5/ros_package_template/LICENSE

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
Software License Agreement (BSD License)
2+
3+
Copyright (c) 2017, Peter Fankhauser
4+
All rights reserved.
5+
6+
Redistribution and use in source and binary forms, with or without
7+
modification, are permitted provided that the following conditions are met:
8+
9+
* Redistributions of source code must retain the above copyright notice, this
10+
list of conditions and the following disclaimer.
11+
12+
* Redistributions in binary form must reproduce the above copyright notice,
13+
this list of conditions and the following disclaimer in the documentation
14+
and/or other materials provided with the distribution.
15+
16+
* Neither the name of Autonomous Systems Lab nor ETH Zurich nor the names
17+
of its contributors may be used to endorse or promote products derived from
18+
this software without specific prior written permission.
19+
20+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21+
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22+
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24+
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25+
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26+
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27+
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28+
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29+
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

‎exercise5/ros_package_template/README.md

Lines changed: 130 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,130 @@
1+
# Package Name
2+
3+
## Overview
4+
5+
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.
6+
7+
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.
8+
9+
The source code is released under a [BSD 3-Clause license](ros_package_template/LICENSE).
10+
11+
**Author(s): Péter Fankhauser
12+
Maintainer: Péter Fankhauser, [email protected]
13+
Affiliation: Autonomous Systems Lab, ETH Zurich**
14+
15+
![Example image](doc/example.jpg)
16+
17+
18+
### Publications
19+
20+
If you use this work in an academic context, please cite the following publication(s):
21+
22+
* 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))
23+
24+
@inproceedings{Fankhauser2015,
25+
author = {Fankhauser, P\'{e}ter and Hutter, Marco},
26+
booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
27+
title = {{PAPER TITLE}},
28+
publisher = {IEEE},
29+
year = {2015}
30+
}
31+
32+
33+
## Installation
34+
35+
### Installation from Packages
36+
37+
To install all packages from the this repository as Debian packages use
38+
39+
sudo apt-get install ros-indigo-...
40+
41+
### Building from Source
42+
43+
#### Dependencies
44+
45+
- [Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics),
46+
- [Eigen] (linear algebra library)
47+
48+
sudo apt-get install libeigen3-dev
49+
50+
51+
#### Building
52+
53+
To build from source, clone the latest version from this repository into your catkin workspace and compile the package using
54+
55+
cd catkin_workspace/src
56+
git clone https://github.com/ethz-asl/ros_package_template.git
57+
cd ../
58+
catkin_make
59+
60+
61+
### Unit Tests
62+
63+
Run the unit tests with
64+
65+
catkin_make run_tests_ros_package_template run_tests_ros_package_template
66+
67+
68+
## Usage
69+
70+
Describe the quickest way to run this software, for example:
71+
72+
Run the main node with
73+
74+
roslaunch ros_package_template ros_package_template.launch
75+
76+
77+
## Nodes
78+
79+
### ros_package_template
80+
81+
Reads temperature measurements and computed the average.
82+
83+
84+
#### Subscribed Topics
85+
86+
* **`/temperature`** ([sensor_msgs/Temperature])
87+
88+
The temperature measurements from which the average is computed.
89+
90+
91+
#### Published Topics
92+
93+
...
94+
95+
96+
#### Services
97+
98+
* **`get_average`** ([std_srvs/Trigger])
99+
100+
Returns information about the current average. For example, you can trigger the computation from the console with
101+
102+
rosservice call /ros_package_template/get_average
103+
104+
105+
#### Parameters
106+
107+
* **`subscriber_topic`** (string, default: "/temperature")
108+
109+
The name of the input topic.
110+
111+
* **`cache_size`** (int, default: 200, min: 0, max: 1000)
112+
113+
The size of the cache.
114+
115+
116+
### NODE_B_NAME
117+
118+
...
119+
120+
121+
## Bugs & Feature Requests
122+
123+
Please report bugs and request features using the [Issue Tracker](https://github.com/ethz-asl/ros_best_practices/issues).
124+
125+
126+
[ROS]: http://www.ros.org
127+
[rviz]: http://wiki.ros.org/rviz
128+
[Eigen]: http://eigen.tuxfamily.org
129+
[std_srvs/Trigger]: http://docs.ros.org/api/std_srvs/html/srv/Trigger.html
130+
[sensor_msgs/Temperature]: http://docs.ros.org/api/sensor_msgs/html/msg/Temperature.html

‎exercise5/ros_package_template/config/default.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
subscriber_topic: /temperature

‎exercise5/ros_package_template/doc/example.jpg

103 KB
Loading

‎exercise5/ros_package_template/include/ros_package_template/Algorithm.hpp

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
#pragma once
2+
3+
namespace ros_package_template {
4+
5+
/*!
6+
* Class containing the algorithmic part of the package.
7+
*/
8+
class Algorithm
9+
{
10+
public:
11+
/*!
12+
* Constructor.
13+
*/
14+
Algorithm();
15+
16+
/*!
17+
* Destructor.
18+
*/
19+
virtual ~Algorithm();
20+
21+
/*!
22+
* Add new measurement data.
23+
* @param data the new data.
24+
*/
25+
void addData(const double data);
26+
27+
/*!
28+
* Get the computed average of the data.
29+
* @return the average of the data.
30+
*/
31+
double getAverage() const;
32+
33+
private:
34+
35+
//! Internal variable to hold the current average.
36+
double average_;
37+
38+
//! Number of measurements taken.
39+
unsigned int nMeasurements_;
40+
};
41+
42+
} /* namespace */

0 commit comments

Comments
 (0)