├── config
    ├── config.yaml~
    └── config.yaml
├── src
    ├── husky_highlevel_controller_node.cpp
    ├── emergency_stop_server.cpp
    ├── emergency_stop_client.cpp
    └── HuskyHighlevelController.cpp
├── package.xml
├── launch
    ├── husky_exercise.launch~
    ├── husky_ekf_bag.launch
    └── husky_pillar.launch
├── include
    └── husky_highlevel_controller
    │   └── HuskyHighlevelController.hpp
├── worlds
    ├── singlePillar.world
    └── driveThrough.world
├── CMakeLists.txt
├── multiplot_config
    └── husky_position.xml
└── README.md


/config/config.yaml~:
--------------------------------------------------------------------------------
1 | husky:
2 |   topic_name: /scan
3 |   queue_size: 1
4 | 


--------------------------------------------------------------------------------
/config/config.yaml:
--------------------------------------------------------------------------------
1 | husky:
2 |   topic_name: /scan
3 |   queue_size: 1
4 |   control_gain: 0.5
5 | 


--------------------------------------------------------------------------------
/src/husky_highlevel_controller_node.cpp:
--------------------------------------------------------------------------------
 1 | #include <ros/ros.h>
 2 | #include "husky_highlevel_controller/HuskyHighlevelController.hpp"
 3 | 
 4 | int main(int argc, char** argv)
 5 | {
 6 |   ros::init(argc, argv, "husky_highlevel_controller");
 7 |   ros::NodeHandle nodeHandle("~");
 8 | 
 9 |   husky_highlevel_controller::HuskyHighlevelController huskyHighlevelController(nodeHandle);
10 | 
11 |   ros::spin();
12 |   return 0;
13 | }
14 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>husky_highlevel_controller</name>
 4 |   <version>0.1.1</version>
 5 |   <description>The husky_highlevel_controller package</description>
 6 |   <maintainer email="yimeng.lu@kaust.edu.sa">Yimeng Lu</maintainer>
 7 |   <license>BSD</license>
 8 |   <author email="yimeng.lu@kaust.edu.sa">Yimeng Lu</author>
 9 | 
10 |   <buildtool_depend>catkin</buildtool_depend>
11 | 
12 |   <depend>roscpp</depend>
13 |   <depend>sensor_msgs</depend>
14 |   <depend>std_stvs</depend>
15 | </package>
16 | 


--------------------------------------------------------------------------------
/launch/husky_exercise.launch~:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |   <include file="$(find husky_gazebo)/launch/husky_empty_world.launch">
 5 |     <arg name="world_name" value="$(find husky_gazebo)/worlds/robocup14_spl_field.world"/>
 6 |     <arg name="laser_enabled" value="true"/>
 7 |   </include>
 8 | 
 9 |   <node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen" >
10 |   </node>
11 |   
12 |   <node name="husky_highlevel_controller" pkg="husky_highlevel_controller" type="husky_highlevel_controller" output="screen" >
13 | 	<rosparam command="load" file="$(find husky_highlevel_controller)/config/config.yaml" />
14 |   </node>
15 | </launch>
16 | 


--------------------------------------------------------------------------------
/launch/husky_ekf_bag.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 |   
 4 |   <param name="use_sim_time" value="true"/>
 5 | 
 6 |   <arg name="husky_gazebo_description" default="$(optenv HUSKY_GAZEBO_DESCRIPTION)"/>
 7 |   <arg name="laser_enabled" default="true"/>
 8 |   <arg name="ur5_enabled" default="false"/>
 9 |   <arg name="kinect_enabled" default="false"/>
10 | 
11 |   <param name="robot_description" command="$(find xacro)/xacro.py '$(arg husky_gazebo_description)'
12 |     laser_enabled:=$(arg laser_enabled)
13 |     ur5_enabled:=$(arg ur5_enabled)
14 |     kinect_enabled:=$(arg kinect_enabled)
15 |     " />
16 | 
17 |   <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization">
18 |     <rosparam command="load" file="$(find husky_control)/config/localization.yaml" />
19 |   </node>
20 | 
21 |   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
22 | 
23 |   <node pkg="rviz" type="rviz" name="rviz" >
24 |   </node>
25 | 
26 | </launch>
27 | 


--------------------------------------------------------------------------------
/src/emergency_stop_server.cpp:
--------------------------------------------------------------------------------
 1 | #include <ros/ros.h>
 2 | #include "husky_highlevel_controller/HuskyHighlevelController.hpp"
 3 | #include <std_srvs/SetBool.h>
 4 | 
 5 | ros::Publisher start_stop_publ; 
 6 | 
 7 | bool stop_(std_srvs::SetBool::Request &request, std_srvs::SetBool::Response &response){
 8 | 	if(request.data == false){		
 9 | 		start_stop_publ.publish(false);
10 | 		response.success = false; 	
11 | 		response.message = "False Todoki mashita! ";
12 | 		// ROS_DEBUG_STREAM("SokaSoka");	
13 | 	}
14 | 	if(request.data == true){
15 | 		start_stop_publ.publish(true);
16 | 		response.success = true; 	
17 | 		response.message = "True Todoki mashita! "; 
18 | 		// ROS_DEBUG_STREAM("NaniNani");	
19 | 	}
20 | 	return true; 
21 | }
22 | 
23 | int main(int argc, char **argv){
24 | 	ros::init(argc, argv, "emergency_stop_server");
25 | 	ros::NodeHandle nh;
26 | 	start_stop_publ = nh.advertise<std_msgs::Bool>("/start_stop", 1);
27 | 	ros::ServiceServer service = nh.advertiseService("emergency_stop", stop_);
28 | 	ros::spin();
29 | 	return 0;
30 | }
31 | 
32 | 
33 | 


--------------------------------------------------------------------------------
/launch/husky_pillar.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | <arg name="laser_enabled" default="true" />
 4 | 
 5 |   <include file="$(find husky_gazebo)/launch/husky_empty_world.launch">
 6 |     <arg name="world_name" value="$(find husky_highlevel_controller)/worlds/singlePillar.world"/>
 7 |     <arg name="laser_enabled" value="$(arg laser_enabled)"/>
 8 |   </include>
 9 | 
10 |   <node name="keyboard_control" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" >
11 |   </node>
12 |   
13 |   
14 |   <node name="husky_highlevel_controller" pkg="husky_highlevel_controller" type="husky_highlevel_controller" output="screen" >
15 | 	<rosparam command="load" file="$(find husky_highlevel_controller)/config/config.yaml" />
16 |   </node>
17 |   
18 | 
19 |   <node pkg="rviz" type="rviz" name="rviz" >
20 |   </node>
21 | 
22 |   <node name = "emergency_stop_server" pkg="husky_highlevel_controller" type="husky_highlevel_controller_server">
23 |   </node>
24 |   
25 |   <!--
26 |   <node name = "emergency_stop_client" pkg="husky_highlevel_controller" type="husky_highlevel_controller_client" >
27 |   </node>
28 |   -->
29 | 
30 | </launch>
31 | 


--------------------------------------------------------------------------------
/src/emergency_stop_client.cpp:
--------------------------------------------------------------------------------
 1 | #include <ros/ros.h>
 2 | #include "husky_highlevel_controller/HuskyHighlevelController.hpp"
 3 | #include <std_srvs/SetBool.h>
 4 | #include <cstdlib>
 5 | 
 6 | int main(int argc, char **argv) {
 7 | 	ros::init(argc, argv, "emergency_stop_client");
 8 | 	if (argc != 2) {
 9 | 		ROS_INFO("usage incorrect");
10 | 		return 1;
11 | 	}
12 | 	ros::NodeHandle nh;
13 | 	
14 | 	ros::ServiceClient client = nh.serviceClient<std_srvs::SetBool>("emergency_stop");
15 | 	std_srvs::SetBool service;
16 | 	//for(int i = 0; i < argc; i++){
17 | 	//	ROS_INFO("%d %s \n", i, argv[i]);
18 | 	//}
19 | 	if(atoi(argv[1]) == 1){
20 | 		ROS_INFO("It's true"); 
21 | 		service.request.data = true; 
22 | 	}else{
23 | 		service.request.data = false;
24 | 	}
25 | 	if (client.call(service)) {
26 | 		// ROS_INFO("Result: %B", (bool)service.response.success);
27 | 		if(service.response.success == true){
28 | 			
29 | 			ROS_DEBUG_STREAM(service.response.message);
30 | 		} else{
31 | 			
32 | 			ROS_DEBUG_STREAM(service.response.message);		
33 | 		}
34 | 	} else {
35 | 		ROS_ERROR("Failed to call service emergency_stop");
36 | 		return 1;
37 | 	}
38 | 	return 0;
39 | } 
40 | 


--------------------------------------------------------------------------------
/include/husky_highlevel_controller/HuskyHighlevelController.hpp:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <ros/ros.h>
 4 | #include <sensor_msgs/LaserScan.h>
 5 | #include <vector>
 6 | #include <string>
 7 | #include <std_msgs/Float32.h>
 8 | #include <math.h>
 9 | #include <geometry_msgs/Twist.h>
10 | #include <visualization_msgs/Marker.h>
11 | #include <std_msgs/Bool.h>
12 | #define pi 3.1419265358
13 | 
14 | using std::vector;
15 | using std::string;
16 | 
17 | namespace husky_highlevel_controller {
18 | 
19 | /*!
20 |  * Class containing the Husky Highlevel Controller
21 |  */
22 | class HuskyHighlevelController {
23 | public:
24 | 	/*!
25 | 	 * Constructor.
26 | 	 */
27 | 	HuskyHighlevelController(ros::NodeHandle& nodeHandle);
28 | 
29 | 	/*!
30 | 	 * Destructor.
31 | 	 */
32 | 	virtual ~HuskyHighlevelController();
33 | 
34 | private:
35 | 	ros::NodeHandle nodeHandle_;
36 | 	void laser_scan_Callback( const sensor_msgs::LaserScan::ConstPtr& laser_scan_msgs );
37 | 	vector<float> laser_scan_distance; 
38 | 	string topic_name; 
39 | 	int queue_size;
40 | 	float minDistance; 
41 | 	float pillarAngle; 
42 | 	void husky_angle_controller(float speed, float angle); 
43 | 	geometry_msgs::Twist cmd_vel_command; 
44 | 	float control_gain; 
45 | 	int direction_index; 
46 | 	void pillar_vis_marker_func(); 
47 | 
48 | 	void start_stop_Callback( const std_msgs::Bool trigger );
49 | 	bool status_; 
50 | 
51 | 	ros::Subscriber laser_scan_subs;
52 | 	ros::Publisher controlled_cmd_vel_publ; 
53 | 	ros::Publisher pillar_vis_publ; 
54 | 	ros::Subscriber start_stop_subs; 
55 |  
56 | };
57 | 
58 | } /* namespace */
59 | 


--------------------------------------------------------------------------------
/worlds/singlePillar.world:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" ?>
 2 | <sdf version="1.4">
 3 |   <world name="default">
 4 |     <!-- A global light source -->
 5 |     <include>
 6 |       <uri>model://sun</uri>
 7 |     </include>
 8 |     <!-- A ground plane -->
 9 |     <include>
10 |       <uri>model://ground_plane</uri>
11 |     </include>
12 |     <!-- Cylinder -->
13 |     <model name='unit_cylinder'>
14 |       <pose frame=''>20 5 0.5 0 -0 0</pose>
15 |       <link name='link'>
16 |         <inertial>
17 |           <mass>1</mass>
18 |           <inertia>
19 |             <ixx>0.145833</ixx>
20 |             <ixy>0</ixy>
21 |             <ixz>0</ixz>
22 |             <iyy>0.145833</iyy>
23 |             <iyz>0</iyz>
24 |             <izz>0.125</izz>
25 |           </inertia>
26 |         </inertial>
27 |         <collision name='collision'>
28 |           <geometry>
29 |             <cylinder>
30 |               <radius>0.2</radius>
31 |               <length>2</length>
32 |             </cylinder>
33 |           </geometry>
34 |           <max_contacts>10</max_contacts>
35 |         </collision>
36 |         <visual name='visual'>
37 |           <geometry>
38 |             <cylinder>
39 |               <radius>0.2</radius>
40 |               <length>2</length>
41 |             </cylinder>
42 |           </geometry>
43 |           <material>
44 |             <script>
45 |               <name>Gazebo/Grey</name>
46 |               <uri>file://media/materials/scripts/gazebo.material</uri>
47 |             </script>
48 |           </material>
49 |         </visual>
50 |         <self_collide>0</self_collide>
51 |         <kinematic>0</kinematic>
52 |       </link>
53 |     </model>
54 | 
55 | 
56 |   </world>
57 | </sdf>
58 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(husky_highlevel_controller)
 3 | 
 4 | ## Use C++11
 5 | add_definitions(--std=c++11)
 6 | 
 7 | ## Find catkin macros and libraries
 8 | find_package(catkin REQUIRED COMPONENTS
 9 |   roscpp
10 |   sensor_msgs
11 |   std_srvs
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 |     roscpp
29 |     sensor_msgs
30 | #  DEPENDS 
31 | )
32 | 
33 | ###########
34 | ## Build ##
35 | ###########
36 | 
37 | ## Specify additional locations of header files
38 | ## Your package locations should be listed before other locations
39 | include_directories(
40 |   include
41 |   ${catkin_INCLUDE_DIRS}
42 | )
43 | 
44 | ## Declare a C++ executable
45 | add_executable(${PROJECT_NAME}
46 |   src/${PROJECT_NAME}_node.cpp
47 |   src/HuskyHighlevelController.cpp
48 | )
49 | ## Specify libraries to link a library or executable target against
50 | target_link_libraries(${PROJECT_NAME}
51 |   ${catkin_LIBRARIES}
52 | )
53 | 
54 | ## Declare a C++ executable
55 | add_executable(${PROJECT_NAME}_client
56 |   src/emergency_stop_client.cpp
57 | )
58 | ## Specify libraries to link a library or executable target against
59 | target_link_libraries(${PROJECT_NAME}_client
60 |   ${catkin_LIBRARIES}
61 | )
62 | 
63 | ## Declare a C++ executable
64 | add_executable(${PROJECT_NAME}_server
65 |   src/emergency_stop_server.cpp
66 | )
67 | ## Specify libraries to link a library or executable target against
68 | target_link_libraries(${PROJECT_NAME}_server
69 |   ${catkin_LIBRARIES}
70 | )
71 | 
72 | 
73 | 


--------------------------------------------------------------------------------
/worlds/driveThrough.world:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" ?>
  2 | <sdf version="1.4">
  3 |   <world name="default">
  4 |     <!-- A global light source -->
  5 |     <include>
  6 |       <uri>model://sun</uri>
  7 |     </include>
  8 |     <!-- A ground plane -->
  9 |     <include>
 10 |       <uri>model://ground_plane</uri>
 11 |     </include>
 12 |     <!-- 2nd Cylinder -->
 13 |     <model name='unit_cylinder'>
 14 |       <pose frame=''>20 5 0.5 0 -0 0</pose>
 15 |       <link name='link'>
 16 |         <inertial>
 17 |           <mass>1</mass>
 18 |           <inertia>
 19 |             <ixx>0.145833</ixx>
 20 |             <ixy>0</ixy>
 21 |             <ixz>0</ixz>
 22 |             <iyy>0.145833</iyy>
 23 |             <iyz>0</iyz>
 24 |             <izz>0.125</izz>
 25 |           </inertia>
 26 |         </inertial>
 27 |         <collision name='collision'>
 28 |           <geometry>
 29 |             <cylinder>
 30 |               <radius>0.2</radius>
 31 |               <length>2</length>
 32 |             </cylinder>
 33 |           </geometry>
 34 |           <max_contacts>10</max_contacts>
 35 |         </collision>
 36 |         <visual name='visual'>
 37 |           <geometry>
 38 |             <cylinder>
 39 |               <radius>0.2</radius>
 40 |               <length>2</length>
 41 |             </cylinder>
 42 |           </geometry>
 43 |           <material>
 44 |             <script>
 45 |               <name>Gazebo/Grey</name>
 46 |               <uri>file://media/materials/scripts/gazebo.material</uri>
 47 |             </script>
 48 |           </material>
 49 |         </visual>
 50 |         <self_collide>0</self_collide>
 51 |         <kinematic>0</kinematic>
 52 |       </link>
 53 |     </model>
 54 | 
 55 |     <!-- 1st Cylinder -->
 56 |     <model name='unit_cylinder'>
 57 |       <pose frame=''>20 8 0.5 0 -0 0</pose>
 58 |       <link name='link'>
 59 |         <inertial>
 60 |           <mass>1</mass>
 61 |           <inertia>
 62 |             <ixx>0.145833</ixx>
 63 |             <ixy>0</ixy>
 64 |             <ixz>0</ixz>
 65 |             <iyy>0.145833</iyy>
 66 |             <iyz>0</iyz>
 67 |             <izz>0.125</izz>
 68 |           </inertia>
 69 |         </inertial>
 70 |         <collision name='collision'>
 71 |           <geometry>
 72 |             <cylinder>
 73 |               <radius>0.2</radius>
 74 |               <length>2</length>
 75 |             </cylinder>
 76 |           </geometry>
 77 |           <max_contacts>10</max_contacts>
 78 |         </collision>
 79 |         <visual name='visual'>
 80 |           <geometry>
 81 |             <cylinder>
 82 |               <radius>0.2</radius>
 83 |               <length>2</length>
 84 |             </cylinder>
 85 |           </geometry>
 86 |           <material>
 87 |             <script>
 88 |               <name>Gazebo/Grey</name>
 89 |               <uri>file://media/materials/scripts/gazebo.material</uri>
 90 |             </script>
 91 |           </material>
 92 |         </visual>
 93 |         <self_collide>0</self_collide>
 94 |         <kinematic>0</kinematic>
 95 |       </link>
 96 |     </model>
 97 | 
 98 |   </world>
 99 | </sdf>
100 | 


--------------------------------------------------------------------------------
/src/HuskyHighlevelController.cpp:
--------------------------------------------------------------------------------
 1 | #include "husky_highlevel_controller/HuskyHighlevelController.hpp"
 2 | 
 3 | namespace husky_highlevel_controller {
 4 | 
 5 | HuskyHighlevelController::HuskyHighlevelController(ros::NodeHandle& nodeHandle) :
 6 |   nodeHandle_(nodeHandle)
 7 | {
 8 | 	nodeHandle.getParam("/husky_highlevel_controller/husky/topic_name", topic_name); 
 9 | 	nodeHandle.getParam("/husky_highlevel_controller/husky/queue_size", queue_size);   
10 | 	nodeHandle.getParam("/husky_highlevel_controller/husky/control_gain", control_gain);   
11 | 	ROS_INFO_STREAM(topic_name << " " << queue_size); 
12 | 	laser_scan_subs = nodeHandle.subscribe( topic_name, queue_size, &HuskyHighlevelController::laser_scan_Callback, this); 
13 | 	controlled_cmd_vel_publ = nodeHandle.advertise<geometry_msgs::Twist>("/cmd_vel", 1); 
14 | 	pillar_vis_publ = nodeHandle.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
15 | 	start_stop_subs =  nodeHandle.subscribe( "/start_stop", 5, &HuskyHighlevelController::start_stop_Callback, this); 
16 | }
17 | 
18 | void HuskyHighlevelController::laser_scan_Callback( const sensor_msgs::LaserScan::ConstPtr& laser_scan_msgs ){
19 | 	minDistance = 30; 
20 | 	direction_index = 0; 
21 | 	laser_scan_distance = laser_scan_msgs->ranges;
22 | 	ROS_INFO_STREAM("msgs received" << std::endl); 
23 | 	for(int i = 0; i < laser_scan_distance.size(); i++){
24 | 		
25 | 		if(minDistance > laser_scan_distance[i]) {
26 | 			minDistance = laser_scan_distance[i];
27 | 			 direction_index = i; 
28 | 		}
29 | 	}
30 | 	pillarAngle =  -0.785 + 1.5*pi/720 * direction_index; 
31 | 
32 | 	// should do sth here to stop/start
33 | 	if(status_ == true){
34 | 		husky_angle_controller(2, pillarAngle); 
35 | 	} else{
36 | 		husky_angle_controller(0, pi/2); 
37 | 	}
38 | 	
39 | 	
40 | 	controlled_cmd_vel_publ.publish(cmd_vel_command);
41 | 	
42 | 
43 | 	pillar_vis_marker_func(); 
44 | 	ROS_INFO_STREAM("The " << direction_index << "st message is chosen in " << laser_scan_distance.size() << " messages");
45 | 	ROS_INFO_STREAM("minDistance detected by Lidar: " << minDistance << ", angle is " << pillarAngle);
46 | 	ROS_INFO_STREAM("Angle_min " << laser_scan_msgs->angle_min << ", Angle_increment " << laser_scan_msgs->angle_increment);
47 | }
48 | 
49 | void HuskyHighlevelController::start_stop_Callback( const std_msgs::Bool trigger ){
50 | 	if(trigger.data == true){
51 | 		status_ = true;
52 | 	}else{
53 | 		status_ = false;
54 | 	} 
55 | }
56 | 
57 | void HuskyHighlevelController::husky_angle_controller(float speed, float angle){
58 | 	// let the husky go at a constant speed
59 | 	// read the angle towards the pillar, and use PID to turn the husky to the pillar (control the angle to 0)
60 | 	cmd_vel_command.linear.x = speed; 
61 | 	cmd_vel_command.angular.z = control_gain * (0 - (angle-pi/2)); 
62 | 	
63 | }
64 | 
65 | void HuskyHighlevelController::pillar_vis_marker_func(){
66 | 	visualization_msgs::Marker marker;
67 | 	marker.header.frame_id = "base_laser";
68 | 	marker.header.stamp = ros::Time();
69 | 	marker.ns = "my_namespace";
70 | 	marker.id = 0;
71 | 	marker.type = visualization_msgs::Marker::SPHERE;
72 | 	marker.action = visualization_msgs::Marker::ADD;
73 | 	marker.pose.position.y = minDistance * cos(pillarAngle);
74 | 	marker.pose.position.x = minDistance * sin(pillarAngle);
75 | 	marker.pose.position.z = 1;
76 | 	marker.pose.orientation.x = 0.0;
77 | 	marker.pose.orientation.y = 0.0;
78 | 	marker.pose.orientation.z = 0.0;
79 | 	marker.pose.orientation.w = 0.0;
80 | 	marker.scale.x = 1;
81 | 	marker.scale.y = 1;
82 | 	marker.scale.z = 1;
83 | 	marker.color.a = 1.0; // Don't forget to set the alpha!
84 | 	marker.color.r = 0.0;
85 | 	marker.color.g = 0.0;
86 | 	marker.color.b = 1.0;
87 | 	//only if using a MESH_RESOURCE marker type:
88 | 	marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
89 | 	pillar_vis_publ.publish( marker );
90 | }
91 | 
92 | HuskyHighlevelController::~HuskyHighlevelController()
93 | {
94 | }
95 | 
96 | } /* namespace */
97 | 


--------------------------------------------------------------------------------
/multiplot_config/husky_position.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="UTF-8"?>
 2 | <rqt_multiplot>
 3 |     <table>
 4 |         <background_color>#ffffff</background_color>
 5 |         <foreground_color>#000000</foreground_color>
 6 |         <link_cursor>false</link_cursor>
 7 |         <link_scale>false</link_scale>
 8 |         <plots>
 9 |             <row_0>
10 |                 <column_0>
11 |                     <axes>
12 |                         <axes>
13 |                             <x_axis>
14 |                                 <custom_title>Untitled Axis</custom_title>
15 |                                 <title_type>0</title_type>
16 |                                 <title_visible>true</title_visible>
17 |                             </x_axis>
18 |                             <y_axis>
19 |                                 <custom_title>Untitled Axis</custom_title>
20 |                                 <title_type>0</title_type>
21 |                                 <title_visible>true</title_visible>
22 |                             </y_axis>
23 |                         </axes>
24 |                     </axes>
25 |                     <curves>
26 |                         <curve_0>
27 |                             <axes>
28 |                                 <x_axis>
29 |                                     <field>pose/pose/position/x</field>
30 |                                     <field_type>0</field_type>
31 |                                     <scale>
32 |                                         <absolute_maximum>1000</absolute_maximum>
33 |                                         <absolute_minimum>0</absolute_minimum>
34 |                                         <relative_maximum>0</relative_maximum>
35 |                                         <relative_minimum>-1000</relative_minimum>
36 |                                         <type>0</type>
37 |                                     </scale>
38 |                                     <topic>/odometry/filtered</topic>
39 |                                     <type>nav_msgs/Odometry</type>
40 |                                 </x_axis>
41 |                                 <y_axis>
42 |                                     <field>pose/pose/position/y</field>
43 |                                     <field_type>0</field_type>
44 |                                     <scale>
45 |                                         <absolute_maximum>1000</absolute_maximum>
46 |                                         <absolute_minimum>0</absolute_minimum>
47 |                                         <relative_maximum>0</relative_maximum>
48 |                                         <relative_minimum>-1000</relative_minimum>
49 |                                         <type>0</type>
50 |                                     </scale>
51 |                                     <topic>/odometry/filtered</topic>
52 |                                     <type>nav_msgs/Odometry</type>
53 |                                 </y_axis>
54 |                             </axes>
55 |                             <color>
56 |                                 <custom_color>#000000</custom_color>
57 |                                 <type>0</type>
58 |                             </color>
59 |                             <data>
60 |                                 <circular_buffer_capacity>1000</circular_buffer_capacity>
61 |                                 <time_frame_length>10</time_frame_length>
62 |                                 <type>0</type>
63 |                             </data>
64 |                             <style>
65 |                                 <lines_interpolate>false</lines_interpolate>
66 |                                 <pen_style>1</pen_style>
67 |                                 <pen_width>1</pen_width>
68 |                                 <render_antialias>false</render_antialias>
69 |                                 <steps_invert>false</steps_invert>
70 |                                 <sticks_baseline>0</sticks_baseline>
71 |                                 <sticks_orientation>2</sticks_orientation>
72 |                                 <type>0</type>
73 |                             </style>
74 |                             <subscriber_queue_size>100</subscriber_queue_size>
75 |                             <title>Traj-XY</title>
76 |                         </curve_0>
77 |                     </curves>
78 |                     <legend>
79 |                         <visible>true</visible>
80 |                     </legend>
81 |                     <plot_rate>30</plot_rate>
82 |                     <title>husky_position</title>
83 |                 </column_0>
84 |             </row_0>
85 |         </plots>
86 |         <track_points>false</track_points>
87 |     </table>
88 | </rqt_multiplot>
89 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # [ETHZ ROS lectures](http://www.rsl.ethz.ch/education-students/lectures/ros.html)
  2 | 
  3 | ## Exercise 1
  4 | 
  5 | - Way to publish to control gazebo husky (press Tab as much as you can)
  6 | 		```
  7 | 		rostopic pub /husky_velocity_controller/cmd_vel geometry_msgs/Twist -r 100 '[0.5,0,0]' '[0,0,0]'
  8 | 		```
  9 | - Need to rebuild and resource when codes get changed
 10 | - If the program can't find something (eg., .world file), look at the launch file, use roscd to find correct path and put it there
 11 | 
 12 | ## [Exercise 2](https://www.ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/ROS2017/exercise2.pdf)
 13 | 
 14 | - yaml file: mind the format and the way to write topic name (/scan with "/")
 15 | - .hpp: include essential header files(msgs, C++ libraries, etc)
 16 | - .cpp: 
 17 | 	- callback: use -> for vector
 18 | 	- CORRECT WAY to write a subscriber and do some processing in callback function
 19 | - .launch: rosparam format
 20 | - Use RViz to visualize stuffs is better in Gazebo
 21 | 
 22 | ## [Exercise 3](https://www.ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/ROS2017/exercise3.pdf)
 23 | 
 24 | ### Husky bumps into pillar
 25 | - Check the exact value of the members in sensor_msgs/LaserScan, it might not be what you imagined. 
 26 | - Need to adjust control gain to get good performance. 
 27 | - CORRECT WAY to write a publisher. 
 28 | 
 29 | ### Visualize pillar observation
 30 | 
 31 | - Another publisher
 32 | - Include headers for message
 33 | - Compute using angle and distance, remember to use correct tf link to visualize in Gazebo
 34 | 
 35 | ## [Exercise 4](https://www.ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/ROS2017/exercise4.pdf)
 36 | 
 37 | - Make sure how nodes communicate by looking at their subs and pubs, and looking at topics as well
 38 | - To launch ekf node, need to load parameters from somewhere. In this example it's in husky_control package
 39 | - To use rqt_multiplot more efficiently, one can save the configuration file and load it directly next time
 40 | - For time, if anything in gazebo is loaded(eg., a world), the time will be set to use_sim_time. otherwise this should be set in the launch file by 
 41 | 
 42 | 		```
 43 | 		<param name="use_sim_time" value="true"/>
 44 | 
 45 | 		```
 46 | 
 47 | And be sure to add --clock while playing rosbag
 48 | 
 49 | 		```
 50 | 		rosbag play data.bag --clock
 51 | 		```
 52 | 
 53 | - For husky visualization in RViz, because it's not loaded via gazebo, we need to do 2 things
 54 | 	- Add a robot_state_publisher node in the launch file(If you notice that there's already /ekf_localization_node publishing to /tf topic. Here's one explanation: because frame and husky config info is not complete only by /ekf's publications)
 55 | 
 56 | 		```
 57 | 		<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 58 | 		```
 59 | 
 60 | 	- Husky's configuration need to be written in launch file. Do this by imitating spawn_husky.launch. Namely, these lines: 
 61 | 
 62 | 		```
 63 | 		  <arg name="husky_gazebo_description" default="$(optenv HUSKY_GAZEBO_DESCRIPTION)"/>
 64 | 		  <arg name="laser_enabled" default="true"/>
 65 | 		  <arg name="ur5_enabled" default="false"/>
 66 | 		  <arg name="kinect_enabled" default="false"/>
 67 | 		
 68 | 		  <param name="robot_description" command="$(find xacro)/xacro.py '$(arg husky_gazebo_description)'
 69 | 		    laser_enabled:=$(arg laser_enabled)
 70 | 		    ur5_enabled:=$(arg ur5_enabled)
 71 | 		    kinect_enabled:=$(arg kinect_enabled)
 72 | 		    " />
 73 | 		```
 74 | 
 75 | ## [Exercise 5](https://www.ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/ROS2017/exercise5.pdf)
 76 | 
 77 | - Implemented a service server to send bool info to a topic, /start_stop to control manual stop of husky
 78 | 		```
 79 | 		rosservice call /emergency_stop "data: true"
 80 | 		```
 81 | - Implemented a service client to communicate with service server, send command from the client and receive feedback as an addition. 
 82 | 		```
 83 | 		rosrun husky_highlevel_controller husky_highlevel_controller_client 1      // for start, anything other than 1 for stop
 84 | 		```
 85 | - Because there are more than one executables now, need to do a lot of modifications to CMakeLists.txt, especially in 
 86 | 	- find_package
 87 | 	- add_executable
 88 | 	- target\_link\_libraries
 89 | 
 90 | - Need to modify package.xml accordingly
 91 | - Created a subs in Husky node to enable/disable husky by sending controller real signals or a constant
 92 | - Corresponding publ is in service server, as it's running all the time
 93 | - Don't know why but ROS\_DEBUG\_MESSAGE only shows while calling service server in console
 94 | - Didn't write client in launch file now. Can be done in future or add this in other parts of the program(automatic stop)
 95 | 
 96 | ## References
 97 | 
 98 | - https://www.ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/ROS2017/exercise5.pdf    
 99 | - https://github.com/fairlight1337/ros_service_examples/blob/master/CMakeLists.txt   
100 | - http://docs.ros.org/jade/api/std_srvs/html/srv/SetBool.html   
101 | - https://www.ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/ROS2017/lecture4.pdf   
102 | - http://wiki.ros.org/ROS/Tutorials/ExaminingServiceClient   
103 | 
104 | 
105 | 
106 | 


--------------------------------------------------------------------------------