Merge pull request #1 from RajPShinde/devel

Working Costmaps & Visualization

README.md

@@ -1,16 +1,39 @@
-# footstep_affordance
+# Footstep Affordance
 
 [![Build Status](https://travis-ci.org/RajPShinde/footstep_affordance.svg?branch=master)](https://travis-ci.org/RajPShinde/footstep_affordance)
 [![License BSD 3-Clause](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](https://github.com/RajPShinde/footstep_affordance/blob/master/LICENSE)
 ---
 
+## Overview
+This is a ROS package to create a cost map of a terrain based on the slope for providing possible foothold locations for legged robots. It uses the 3D-Occupancy from the octomap to create a height map of the terrain, this height map is used to obtain slope at each point using PCA.  
+
+<p align="center">
+<img src="data/costmap.png"/>
+</p>
+<p align="center">
+<img src="data/steps.png"/>
+</p>
+
 ## Dependencies
-1. Ubuntu 18.04
-2. C++ 11/14/17
-3. Eigen
-4. ROS Melodic
+1. Ubuntu 16.04 and above
+2. ROS Kinetic and above
+3. C++ 11/14/17
+4. octomap-ros
+5. octomap-server
+4. Eigen
+
+## Subscriptions
+1. ```/octomap_binary```
+
+## Publications
+1. ```/heightMapVisualization```
+2. ```/costMapVisualization```
+
+## Params
+1. ```world_frame```(default:/odom)
+2. ```robot_frame```(default:/base_link)
 
-## References
+## Citations
 C. Mastalli, I. Havoutis, M. Focchi, D. G. Caldwell, C. Semini, Motion planning for challenging locomotion: a study of decoupled and coupled approaches, IEEE International Conference on Robotics and Automation (ICRA), 2017
 
 ## Disclaimer

include/features.hpp

@@ -16,6 +16,8 @@ class Feature {
 
     	void computeSlopeCost(double &cost, costmap::TerrainData &terrainParameters);
 
+    	void computeHeightDeviationCost(double& cost, costmap::TerrainData &terrainParameters);
+
 	private:
 		double flatThreshold_, steepThreshold_;
 		double maxCost_ = 1; 

include/footstepAffordance.hpp

@@ -44,17 +44,17 @@ class FootstepAffordance
     private:    
 
         const double distanceFromRobot = 0.3;
-        double maxX = 2; 
-        double minX = 1;
-        double maxY = 0.15;
-        double minY = -0.15; 
-        double maxZ = 0.8;
-        double minZ = -0.8;
+        double maxX = 1.0; 
+        double minX = 0.24;
+        double maxY = 0.18;
+        double minY = -0.18; 
+        double maxZ = 0.4;
+        double minZ = -0.4;
         TerrainData terrainParameters;
         int depth_ = 16;
         bool firstRun_ = false;
-        int neighbourMaxX, neighbourMaxY, neighbourMaxZ = 2;
-        int neighbourMinX, neighbourMinY, neighbourMinZ = -2;
+        int neighbourMaxX, neighbourMaxY, neighbourMaxZ = 1;
+        int neighbourMinX, neighbourMinY, neighbourMinZ = -1;
         terrainFeature::Feature features;
 };
 

src/features.cpp

@@ -1,9 +1,10 @@
 #include <features.hpp>
+#include <ros/ros.h>
 
 namespace terrainFeature
 {
 
-Feature::Feature() : flatThreshold_(M_PI / 180), steepThreshold_(60 * (M_PI / 180)) {
+Feature::Feature() : flatThreshold_(8 * (M_PI / 180)), steepThreshold_(60 * (M_PI / 180)) {
 }
 
 Feature::~Feature() {
@@ -12,17 +13,24 @@ Feature::~Feature() {
 void Feature::computeSlopeCost(double &cost, costmap::TerrainData &terrainParameters) {
 
 	double slope = fabs(acos((double) terrainParameters.normal(2)));
+	ROS_INFO_STREAM(slope);
 	if(slope < flatThreshold_) {
 		cost = 0;
 	}
 	else if(slope < steepThreshold_) {
-		cost = -log(1-(slope - flatThreshold_)/(flatThreshold_- steepThreshold_));
+		cost = -log(1-(slope - flatThreshold_)/(steepThreshold_ - flatThreshold_));
+		// ROS_INFO_STREAM(cost);
 		if(maxCost_ < cost)
 			cost = maxCost_;
 	}
 	else {
 		cost = maxCost_;
 	}
+	ROS_INFO_STREAM(cost);
 }
 
+// void Feature::computeHeightDeviationCost(double& cost, costmap::TerrainData &terrainParameters) {
+
+// }
+
 }

src/footstepAffordance.cpp

@@ -260,20 +260,22 @@ void FootstepAffordance::calculateCost(octomap::OcTree* octomap, const octomap::
 
 	octomap::OcTreeKey neighbourKey;
 	octomap::OcTreeNode* neighbourNode = surfaceCellNode;
+	bool yq = false;
 	ROS_ERROR_STREAM("Neighbours");
-	for(int z = neighbourMinZ; z<=neighbourMaxZ; z++) {
-		for(int y = neighbourMinY; y<=neighbourMaxY; y++) {
-			for(int x = neighbourMinX; x<=neighbourMaxX; x++) {
+	for(int z = -1; z<=1; z++) {
+		for(int y = -1; y<=1; y++) {
+			for(int x = -1; x<=1; x++) {
 				neighbourKey[0] = surfaceCellKey[0] + x;
 				neighbourKey[1] = surfaceCellKey[1] + y;
 				neighbourKey[2] = surfaceCellKey[2] + z;
 				neighbourNode = octomap->search(neighbourKey, depth_);
-				ROS_ERROR_STREAM(z);
+				yq =true;
+				//ROS_ERROR_STREAM(z);
 				// Check if this neighbour exists
 				if(neighbourNode) {
-					ROS_ERROR_STREAM("Found1");
+					// ROS_ERROR_STREAM("Found1");
 					if(octomap->isNodeOccupied(neighbourNode)) {
-						ROS_ERROR_STREAM("Found2");
+						// ROS_ERROR_STREAM("Found2");
 						Eigen::Vector3d neighbourCellXYZ;
 						octomap::point3d neighbourCellPoint;
 						neighbourCellPoint = octomap->keyToCoord(neighbourKey, depth_);
@@ -287,24 +289,26 @@ void FootstepAffordance::calculateCost(octomap::OcTree* octomap, const octomap::
 			}
 		}
 	}
-
-    ROS_ERROR_STREAM("Found Neighbours");
+	if(yq)
+		ROS_WARN_STREAM("HAA andar gaya tha");
 	if(atLeastOneNeighbour) {
 		//compute cost
+		ROS_ERROR_STREAM("At Least One Neighbour");
 		EIGEN_ALIGN16 Eigen::Matrix3d covarianceMatrix;
-		if(allNeighboursPosition.size()<3) {
-			computeMeanAndCovariance(terrainParameters.meanPosition, covarianceMatrix, allNeighboursPosition);
-		}
-		else
+		if(allNeighboursPosition.size()<3 || computeMeanAndCovariance(terrainParameters.meanPosition, covarianceMatrix, allNeighboursPosition)==0){
+			ROS_ERROR_STREAM("<3");
 			return;
+		}
+		ROS_WARN_STREAM(allNeighboursPosition.size());
 		terrainParameters.centroidPosition(0) = allNeighboursPosition[0](0);
-		terrainParameters.centroidPosition(0) = allNeighboursPosition[0](1);
-		terrainParameters.centroidPosition(0) = allNeighboursPosition[0](2);
+		terrainParameters.centroidPosition(1) = allNeighboursPosition[0](1);
+		terrainParameters.centroidPosition(2) = allNeighboursPosition[0](2);
 
 		solvePlaneParameters(terrainParameters.normal, terrainParameters.curvature, covarianceMatrix);
 	}
 
 	ROS_ERROR_STREAM("Cost");
+	ROS_ERROR_STREAM(terrainParameters.normal);
 	double cost, weight, totalCost = 0;
 	// for(auto i: features) {
 		features.computeSlopeCost(cost, terrainParameters);

src/interface.cpp

@@ -12,8 +12,8 @@ Interface::Interface(ros::NodeHandle& nh) : interface_(nh), robotFrame_("base_li
     tfOctomapSub_ = new tf::MessageFilter<octomap_msgs::Octomap>(*octomapSub_, tf_listener_, worldFrame_, 5);
     tfOctomapSub_->registerCallback(boost::bind(&Interface::octomapCallback, this, _1));
 
-    visualizeHeightMapPub = vis_.advertise<visualization_msgs::MarkerArray>( "/heightMap", 10 );
-    visualizeCostMapPub = vis_.advertise<visualization_msgs::MarkerArray>( "/costMap", 10 );
+    visualizeHeightMapPub = vis_.advertise<visualization_msgs::MarkerArray>( "/heightMapVisualization", 10 );
+    visualizeCostMapPub = vis_.advertise<visualization_msgs::MarkerArray>( "/costMapVisualization", 10 );
 
 }
 
@@ -141,7 +141,19 @@ void Interface::visualizeCostMap(std::map<std::pair<double, double>, double> &hM
         marker.markers[i].color.a = 1.0; 
         marker.markers[i].color.r = 0.0;
         marker.markers[i].color.g = 0.0;
-        marker.markers[i].color.b = 1.0/it->second;
+        // if(it->second==0){
+        // marker.markers[i].color.r = 0.0;
+        // marker.markers[i].color.g = 1.0;
+        // marker.markers[i].color.b = 0.0;
+        // }
+        // else{
+        // marker.markers[i].color.r = it->second;
+        // marker.markers[i].color.g = 0.0;
+        // marker.markers[i].color.b = 0.0;   
+        // }
+        marker.markers[i].color.r = (it->second > 0.5 ? 1.0 : 2*it->second);
+        marker.markers[i].color.g = (it->second > 0.5 ? 1-2*(it->second-0.5) : 1.0);
+        marker.markers[i].color.b = 0.0; 
         i++;
     }
     visualizeCostMapPub.publish(marker);