node1.cpp

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <math.h> 

geometry_msgs::Twist twist;
geometry_msgs::Twist desired;
bool newMsg, newDesired;
#define PI 3.14159265

void desCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
  ROS_INFO_STREAM("DESIRED CALLBACK");
  twist.angular = msg->angular;
  twist.linear = msg->linear;
  newMsg = true;
}

void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& msg){
  ROS_INFO_STREAM("LIDAR CALLBACK");
  int angleWithMaxDistance = 0;
  int maxDistance = 0.01;
  int currentDistance;
  for(int i=0; i<msg->ranges.size();i++){
    currentDistance = msg->ranges.at(i);
    if(currentDistance > maxDistance){
      maxDistance = msg->ranges.at(i);
      angleWithMaxDistance = i;
    }
    if(msg->ranges.at(135) > 4){
      angleWithMaxDistance = 135;
      ROS_INFO_STREAM("STR8 AHEAD CLEAR BREAKING");  
      break;
    }
  }
  ROS_INFO_STREAM("angleWithMaxDistance " << angleWithMaxDistance);
  if(angleWithMaxDistance > 130 && angleWithMaxDistance < 140){
    ROS_INFO_STREAM("STR8 AHEAD");
    //If straight ahead is clearest
    desired.linear.x = 0.2;
  } else {
    //If clearest is at an angle
    ROS_INFO_STREAM("ANGLED");
    desired.linear.x = 0.1;
    int angle = 135 - angleWithMaxDistance;
    desired.angular.z = std::sin(angle * PI / 180.0 );
  }
  //if str8 ahead is less than 0.69m, back up and rotate 
  if(msg->ranges.at(135) < 0.69){
    ROS_INFO_STREAM("DUMPA TRUCK");
      desired.linear.x = -0.3;
      // desired.angular.z = -0.3;
    }
  newDesired = true;
}

int main(int argc, char **argv)
{

  ros::init(argc, argv, "node1");
  ros::NodeHandle n;

  ros::Subscriber des_sub = n.subscribe("des_vel", 1000, desCallback);
  ros::Subscriber lidar_sub = n.subscribe("/robot0/laser_1", 1000, lidarCallback);
  ros::Publisher cmd_vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
  ros::Publisher robot_vel_pub = n.advertise<geometry_msgs::Twist>("/robot0/cmd_vel", 1000);
  

  while(ros::ok()){
    // ROS_INFO_STREAM("SPINNING");
    if(newDesired){
      robot_vel_pub.publish(desired);
      newDesired = false;
    }    
    if(newMsg){
      cmd_vel_pub.publish(twist);
      newMsg = false;
    }
    
    ros::spinOnce();
  }
  

  return 0;
}