#include "ros/ros.h"
#include "std_msgs/String.h"
#include <geometry_msgs/Twist.h>
#include <iostream>

bool flag=0;
geometry_msgs::Twist cmd_vel_msg;


void doMsg(const std_msgs::String::ConstPtr& msg_p){
    
    flag=1;
    std::cout<<flag<<std::endl;
}

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");

    ros::init(argc,argv,"listener");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe<std_msgs::String>("/room_exploration/room_exploration_server/stereo_msg",10,doMsg);
    ros::Publisher cmd_vel_pub_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 100);
    
    ros::Rate rate(10);

    while(ros::ok())
    {
        if(flag==1)
	    {
            ROS_INFO("enter");
            flag=0;

            cmd_vel_msg.angular.z = -1;
            for (int i = 0; i < 17; ++i) {  
            cmd_vel_pub_.publish(cmd_vel_msg);
            rate.sleep();
            }
            cmd_vel_msg.angular.z = 0;
            cmd_vel_msg.linear.x = 0.2; 

            for (int i = 0; i < 40; ++i) {  
                cmd_vel_pub_.publish(cmd_vel_msg);
                rate.sleep();
            }
            cmd_vel_msg.linear.x = 0; 
            cmd_vel_msg.angular.z = 1;
            for (int i = 0; i < 16; ++i) {  
            cmd_vel_pub_.publish(cmd_vel_msg);
            rate.sleep();
            }
            ROS_INFO("stop!!");
            cmd_vel_msg.angular.z = 0;
            cmd_vel_msg.linear.x = 0; 
            cmd_vel_pub_.publish(cmd_vel_msg);
        }

        ros::spinOnce();
    }
            // cmd_vel_msg.angular.z = 1;
            // for (int i = 0; i < 19; ++i) {  
            // cmd_vel_pub.publish(cmd_vel_msg);
            // rate.sleep();
            // }
    

    return 0;
}