Fub Controller Example

[rafacastalla]

Could you publish a simple example to test the path controller, please? Or correct this code, please?

I'm trying to do some code to test it.

#include <ros/ros.h>
#include <fub_trajectory_msgs/Trajectory.h>
#include <fub_trajectory_msgs/TrajectoryPoint.h>
#include

/** Starting point for the node. It instantiates the nodelet within the node
** (alternatively the nodelet could be run in a standalone nodelet manager).
**
** @PARAM argc
** @PARAM argv
** @return
**
** @InGroup @@
*/
int main(int argc, char ** argv)
{
ros::init(argc, argv, "planner");

ros::NodeHandle n;

ros::Publisher chatter_pub = n.advertise<fub_trajectory_msgs::Trajectory>("model_car/trajectory", 1000);
  	
fub_trajectory_msgs::Trajectory msg;
fub_trajectory_msgs::TrajectoryPoint points[2];

msg.header.stamp = ros::Time::now();
msg.header.frame_id = "odom";
msg.header.seq = 0;

msg.trajectory.resize(2);

msg.trajectory[0].velocity.linear.x = 10;
msg.trajectory[0].velocity.linear.y = 10;

msg.trajectory[0].acceleration.linear.x = 2;
msg.trajectory[0].acceleration.linear.y = 2;

msg.trajectory[0].time_from_start = ros::Duration(1.0);

msg.trajectory[1].pose.position.x = 100;
msg.trajectory[1].pose.position.y = 100;

msg.trajectory[1].velocity.linear.x = 10;
msg.trajectory[1].velocity.linear.y = 10;

msg.trajectory[1].time_from_start = ros::Duration(20.0);

std::cout << msg << "\n";


chatter_pub.publish(msg);

}

[AutoModelCar]

There is a bag file contains a trajectory in the link below:

http://ftp.imp.fu-berlin.de/pub/autonomos/data/modelcar/rosbags/

You can play the rosbag and then run the fub_controller.