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

include

include

include

include

include

include

include

include

include

include

include

include

// Representation (RVIZ)

include

/** * 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 goalx, double goaly);

private:

ros::NodeHandle nh_;

//2D robot pose double x,y,theta; // Scan sensormsgs::LaserScan datascan;

//Publisher and subscribers ros::Publisher velpub; ros::Subscriber kinectsub; ros::Subscriber posesub;

//!Publish the command to the turtlebot void publish(double angularvel, double linearvel);

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

};

Turtlebot::Turtlebot() { velpub = nh.advertise<geometrymsgs::Twist>("cmd_vel", 1);

posesub = nh.subscribe("/robotposeekf/odomcombined",1,&Turtlebot::receivePose,this);

kinectsub = 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 sensormsgs::LaserScan& msg) { datascan=msg; // Different variables used to detect obstacles

}

std::vector loadPlan(const char *filename); void visualizePlan(const std::vector &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 " << std::endl; return 0; }

std::vector plan = loadPlan(argv[1]);

unsigned int cont_wp = 0;

ros::Rate looprate(20); ros::Publisher markerpub = n.advertise("visualization_marker", 4);

/* Complete the code to follow the path, calling adequately to the command function */

while (ros::ok() && cont_wp < plan.size()) {

ros::spinOnce();

visualizePlan(plan, marker_pub );



}
loop_rate.sleep();

}

return 0;

}

std::vector loadPlan(const char *filename) { std::vector plan; double x,y;

std::ifstream is(filename);

while (is.good()) { is >> x; if (is.good()) { is >> y; geometrymsgs::PoseStamped currway; currway.pose.position.x = x; currway.pose.position.y = y; plan.pushback(currway); ROSINFO("Loaded waypoint (%f, %f).", x , y); } } ROSINFO("Plan loaded successfully."); return plan; }

void visualizePlan(const std::vector< geometrymsgs::PoseStamped >& plan, ros::Publisher &markerpub ) { ros::NodeHandle n;

uint32t shape = visualizationmsgs::Marker::CUBE;

for (unsigned int i = 0; i < plan.size(); i++) { visualizationmsgs::Marker marker; // Set the frame ID and timestamp. See the TF tutorials for information on these. marker.header.frameid = "/odom_combined"; // This is the default fixed frame in order to show the move of the robot marker.header.stamp = ros::Time::now();

// Set the namespace and id for this marker.  This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
marker.ns = "plan";
marker.id = i;

// Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = shape;

// Set the marker action.  Options are ADD and DELETE
marker.action = visualization_msgs::Marker::ADD;

// Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
marker.pose.position.x = plan[i].pose.position.x;
marker.pose.position.y = plan[i].pose.position.y;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;

// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;

// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;

marker.lifetime = ros::Duration(); // Eternal marker

// Publish the marker
marker_pub.publish(marker);

} } `