Sequence movement with delay

edit

Hi there,

I am trying to put a delay between a movement of turtlebot so that I can execute the movement step by step. However, the behavior of the turtlebot is different from what I expected. It's only execute the first movement and skip the other command. Could anybody help me out here? Here I attached my sample of program. Thank you.

#include <ros/ros.h>
#include <stdlib.h>
#include <iostream.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <numeric>

using namespace std;

ros::Publisher pub_vel;
geometry_msgs::Twist cmdvel;
geometry_msgs::Pose Curr_Loc;

void odomCB(const nav_msgs::Odometry::ConstPtr& msg)
{
    Curr_Loc = msg->pose.pose;
    double x=msg->pose.pose.position.x;
    double y=msg->pose.pose.position.y;
    double Z=msg->pose.pose.orientation.z;
    double W=msg->pose.pose.orientation.w;

    cout<<"Current location: "<<x<<", "<<y<<"  Orientation  "<<Z<<", "<<W<<endl;

    cmdvel.angular.z=0.0;
    cmdvel.linear.x=0.2;
    usleep(300000); //delay
    cmdvel.angular.z=-0.2;
    cmdvel.linear.x=0.0;
    usleep(300000); //delay

pub_vel.publish(cmdvel);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "sequence move");
    ros::NodeHandle n;

    pub_vel=n.advertise<geometry_msgs::Twist>("cmd_vel",1);
    pub_vel.publish(cmdvel);

    ros::Subscriber Sub_Odom=n.subscribe("odom",1,odomCB);  //subcribe
    ros::spin();
    return 0;
}