Simple avoiding obstacles using Turtlebot simulator and kinect(C++)

edit

Hello guys,

I'm having problems with simple navigation to goal with avoiding obstacles on the road. I'm all new to ROS and C++ even more, can you help me with understanding the code? `Any suggestions how could I do it? Path to follow is read from file and has x,y coordinates in matrix and avoiding done using messages from laserscan kinect.

#include <ros ros.h=""> #include <geometry_msgs twist.h=""> #include <tf tf.h="">

#include <geometry_msgs posestamped.h=""> #include <geometry_msgs posewithcovariancestamped.h=""> #include <tf transform_datatypes.h=""> #include <linearmath btmatrix3x3.h=""> #include <stdio.h>

#include <math.h> #include <vector> #include <fstream> #include <sstream> #include <sensor_msgs laserscan.h="">

// Representation (RVIZ) #include <visualization_msgs marker.h="">

/** * Our class to control the robot * It has members to store the robot pose, and * methods to control the robot by publishing data */ class Turtlebot { public: Turtlebot();

/* * This function should command the robot to reach the goal * It should compute the commands to the robot by knowing the current position * and the goal position. * This function will return true only if the goal has been reached. */ bool command(double goal_x, double goal_y);

private:

ros::NodeHandle nh_;

//2D robot pose double x,y,theta; // Scan sensor_msgs::LaserScan data_scan;

//Publisher and subscribers ros::Publisher vel_pub_; ros::Subscriber kinect_sub_; ros::Subscriber pose_sub_;

//!Publish the command to the turtlebot void publish(double angular_vel, double linear_vel);

//!Callback for robot position void receivePose(const geometry_msgs::PoseWithCovarianceStampedConstPtr & pose); //!Callback for kinect void receiveKinect(const sensor_msgs::LaserScan & laser_kinect);

};

Turtlebot::Turtlebot() { vel_pub_ = nh_.advertise<geometry_msgs::twist>("cmd_vel", 1);

pose_sub_ = nh_.subscribe("/robot_pose_ekf/odom_combined",1,&Turtlebot::receivePose,this);

kinect_sub_ = nh_.subscribe("/scan",1,&Turtlebot::receiveKinect,this);

}

bool Turtlebot::command(double gx, double gy)
{

double linear_vel=0.0;
double angular_vel=0.0;
double dist_x, dist_y, distance, prod;
double angulo;
bool ret_val = false;
//sensor_msgs::LaserScan scan_data; 

/**
* This should be completed
*/






publish(angular_vel,linear_vel);

return ret_val; }

//Publish the command to the turtlebot void Turtlebot::publish(double angular, double linear)
{ geometry_msgs::Twist vel; vel.angular.z = angular; vel.linear.x = linear;

//std::cout << "Velocidades: " << vel.linear.x << ", " << vel.angular.z << std::endl;

vel_pub_.publish(vel);

return; }

//Callback for robot position void Turtlebot::receivePose(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) {

x=msg->pose.pose.position.x;
y=msg->pose.pose.position.y;

theta=tf::getYaw(btQuaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w));

//std::cout << "Pose: " << x << ", " << y << " ," << theta << std::endl;

}

//Callback for robot position void Turtlebot::receiveKinect(const sensor_msgs::LaserScan& msg) { data_scan=msg; // Different variables used to detect obstacles

}

std::vector<geometry_msgs::posestamped> loadPlan(const char *filename); void visualizePlan(const std::vector<geometry_msgs::posestamped> &plan, ros::Publisher &marker_pub );

int main(int argc, char** argv) { ros::init(argc, argv, "robot_control"); Turtlebot robot; ros::NodeHandle n;

if(argc<2) { std::cout << "Insufficient number of parameters" << std::endl; std::cout << "Usage: robot_control <filename>" << std::endl; return 0; }

std::vector<geometry_msgs::posestamped> plan = loadPlan(argv[1]);

unsigned int cont_wp = 0;

ros::Rate loop_rate(20); ros::Publisher marker_pub = n.advertise<visualization_msgs::marker>("visualization_marker", 4 ... (more)