Week2.md

---

---

Week2

Downloading smb_highlevel_controller

Download via .zip given at the course website, and we extract it to ~/catkin_ws/src.
We can open and inspect the package.xml and CMakeLists.txt via

cd ~/catkin_ws/src/smb_highlevel_controller
nano package.xml
nano CMakeLists.txt

Task: Modifying the node, and its header file to do the given task

First, we modify the header file to make SmbHighLevelController class

#pragma once
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <string.h>
using namespace std;
namespace smb_highlevel_controller {
class SmbHighlevelController {
public:
SmbHighlevelController (ros::NodeHandle& nodeHandle);
virtual ~SmbHighlevelController();
	private:
		bool readParameters();
		void scanCallback(const sensor_msgs::LaserScan::ConstPtr&amp; msg);

		ros::NodeHandle nodeHandle_;
		ros::Subscriber scanSubscriber_;
		ros::Publisher twistPublisher_;
		string scanTopic_;
		int subscriberQueueSize_;
		string twistTopic_;
		int publisherQueueSize_;

};
} /* namespace */

once the class is made, we make the changes to SmbHighlevelController.cpp:

namespace smb_highlevel_controller {
SmbHighlevelController::SmbHighlevelController(ros::NodeHandle& nodeHandle) :
nodeHandle_(nodeHandle),
scanTopic_("/scan"),
subscriberQueueSize_(10),
twistTopic_("/twist"),
publisherQueueSize_(10)
{
if (!readParameters()){
ROS_ERROR("CANT READ PARAMETERS");
ros::requestShutdown;
}
scanSubscriber_ = nodeHandle_.subscribe(scanTopic_, subscriberQueueSize_, &SmbHighlevelController::scanCallback, this);
twistPublisher_ =  nodeHandle_.advertise<geometry_msgs::Twist>(twistTopic_, publisherQueueSize_);
}
SmbHighlevelController::~SmbHighlevelController()
{
}
bool SmbHighlevelController::readParameters() {
if(!nodeHandle_.getParam("/smb_high_level_controller/scan_subscriber_queue_size", subscriberQueueSize_)){
return false;
}
if(!nodeHandle_.getParam("/smb_highlevel_controller/scan_subscriber_topic_name", scanTopic_)) {
return false;
}
if(!nodeHandle_.getParam("/smb_highlevel_controller/twist_publisher_queue_size", publisherQueueSize_)) {
return false;
}
if(!nodeHandle_.getParam("/smb_high_level_controller/twist_publisher_topic_name", twistTopic_)){
return false;
}
return true;
}
void SmbHighlevelController::scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg) {
float minDistance = 100.0;
int minDistanceIndex = 0;
int i = 0;
float angle = msg-&gt;angle_min;

while (angle &lt;= msg-&gt;angle_max ) {
	if(msg-&gt;ranges [i]&lt;minDistance) {
		minDistance= msg-&gt;ranges [i];
		minDistanceIndex= i;
	}
	i++;
	angle += msg-&gt;angle_increment;
}

ROS_INFO("Min Distance: %f, angle: %f", minDistance, msg-&gt;angle_min+minDistanceIndex*msg-&gt;angle_increment);

}
}/* namespace */

In this we implement the following:
the while loop is such that it is called whenever a new laser scan message is received. It finds the minimum distance in the laser scan and publishes a twist message that rotates the robot away from that direction.

Finally, the smb_highlevel_Controller_node.cpp is:

#include <ros/ros.h>
#include "smb_highlevel_controller/SmbHighlevelController.hpp"
int main(int argc, char** argv) {
ros::init(argc, argv, "smb_highlevel_controller");
ros::NodeHandle nodeHandle("~");
smb_highlevel_controller::SmbHighlevelController smbHighlevelController(nodeHandle);
ros::spin();
return 0;
}

Now for the parameter file, I used a .yaml file as it was easier to read, and I stored it in a made directory : ~/catkin_ws/src/smb_highlevel_controller/param
the file is as follows :

scan_subscriber_queue_size : 1
scan_subscriber_topic_name : /scan
twist_publisher_queue_size : 1
twist_publisher_topic_name : /scan

Modifying the launch file

the new launch file is:

<?xml version="1.0" encoding="utf-8"?>
<launch>
 <arg name="world_name" default="big_map_summer_school" />
 <arg name="laser" default="true" />
 <include file="~/Workspaces/smb_ws/src/smb_gazebo/launch/smb_gazebo.launch">
   <arg name="world" value="$(arg world_name)" />
   <arg name="laser_enabled" value="$(arg laser)" />
 </include>
 <node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" />
 <node name="smb_highlevel_controller" pkg="smb_highlevel_cotroller" type="smb_highlevel_controller" output="screen">
   <rosparam command="load" file="$(find smb_highlevel_controller)/param/param.yaml" />
 </node>
 <node name="rviz" pkg="rviz" type="rviz" output="screen" />
</launch>

But due to constant error, I could not proceed further