int main(int argc, char **argv) {
ros::init(argc, argv, "des_state_publisher");
ros::NodeHandle nh;
//instantiate a desired-state publisher object
DesStatePublisher desStatePublisher(nh);
//dt is set in header file pub_des_state.h
ros::Rate looprate(1 / dt); //timer for fixed publication rate
desStatePublisher.set_init_pose(0,0,0); //x=0, y=0, psi=0
//put some points in the path queue--hard coded here
// main loop; publish a desired state every iteration
while (ros::ok()) {
desStatePublisher.pub_next_state();
ros::spinOnce();
looprate.sleep(); //sleep for defined sample period, then do loop again
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "des_state_publisher");
ros::NodeHandle nh;
ros::Subscriber odom_subscriber = nh.subscribe("odom", 1, odomCallback);
ros::Publisher start_publisher = nh.advertise<std_msgs::Bool>("start_trigger", 1, true); // talks to the robot!
std_msgs::Bool start_cmd;
RobotCommander robot(&nh);
//instantiate a desired-state publisher object
DesStatePublisher desStatePublisher(nh);
//dt is set in header file pub_des_state.h
ros::Rate looprate(1 / dt); //timer for fixed publication rate
ROS_WARN("You can now move the robot, input anything to start");
getchar();
ROS_INFO("Waiting for good amcl particles");
robot.turn(M_PI*2);
//ROS_INFO("Waiting for odometery message to start...");
while (!is_init_orien) {
ros::spinOnce(); //wait for odom callback
}
ROS_INFO("got odometery with x = %f, y = %f, th = %f", g_current_pose.position.x, g_current_pose.position.y, quat2ang(g_current_pose.orientation));
desStatePublisher.set_init_pose(g_current_pose.position.x, g_current_pose.position.y, quat2ang(g_current_pose.orientation)); //x=0, y=0, psi=0
double gcpX = g_current_pose.position.x;
double gcpY = g_current_pose.position.y;
double gcpTh = quat2ang(g_current_pose.orientation);
start_cmd.data = true;
start_publisher.publish(start_cmd);
if (argc > 1 && ( strcmp(argv[1], "jinx") == 0 )) {
double x = 5.6 + gcpX;
double y = -12.4 + gcpY;
double v = 6.1 + 1.8 - 0.3; // 20 tiles + 3yd - 1ft for safety
desStatePublisher.append_path_queue(x, gcpY, gcpTh );
desStatePublisher.append_path_queue(x, gcpY, gcpTh-M_PI/2);
desStatePublisher.append_path_queue(x, y, gcpTh-M_PI/2);
desStatePublisher.append_path_queue(x-v, y, gcpTh-M_PI );
} else if (argc > 1 && ( strcmp(argv[1], "test") == 0 )) {
desStatePublisher.append_path_queue(0.5, 0.0, 0.0);
} else if (argc > 1 && ( strcmp(argv[1], "circle_out") == 0 )) {
double circle_length = 2.0;
/* clockwise*/
desStatePublisher.append_path_queue(circle_length, 0.0, 0.0);
desStatePublisher.append_path_queue(circle_length, circle_length, -M_PI/2);
desStatePublisher.append_path_queue(-circle_length, circle_length, -M_PI);
desStatePublisher.append_path_queue(-circle_length, -circle_length, M_PI/2);
desStatePublisher.append_path_queue(circle_length, -circle_length, 0.0);
desStatePublisher.append_path_queue(circle_length, circle_length, -M_PI/2);
/*
desStatePublisher.append_path_queue(circle_length, 0.0, -M_PI/2);
desStatePublisher.append_path_queue(circle_length, -circle_length, -M_PI);
desStatePublisher.append_path_queue(-circle_length, -circle_length, M_PI/2);
desStatePublisher.append_path_queue(-circle_length, circle_length, 0.0);
*/
desStatePublisher.append_path_queue(0.0, circle_length, M_PI/2);
desStatePublisher.append_path_queue(0.0, 7.0, M_PI);
desStatePublisher.append_path_queue(-9.0, 7.0, -M_PI/2);
desStatePublisher.append_path_queue(-9.0, 0.0, -M_PI/2);
} else if (argc > 1 && ( strcmp(argv[1], "out") == 0 )) {
desStatePublisher.append_path_queue(0.0, 7.0, -M_PI/2);
desStatePublisher.append_path_queue(-9.0, 7.0, -M_PI);
desStatePublisher.append_path_queue(-9.0, 0.0, M_PI/2);
} else {
desStatePublisher.append_path_queue(0.0, 0.0, 0.0);
}
// main loop; publish a desired state every iteration
while (ros::ok()) {
desStatePublisher.pub_next_state();
ros::spinOnce();
looprate.sleep(); //sleep for defined sample period, then do loop again
}
}
