#include <ros/ros.h>
#include <riki_msgs/ctrl.h>

bool control(riki_msgs::ctrl::Request &req, riki_msgs::ctrl::Response &res)
{
    if(req.cmd==0)
    {
        ROS_INFO("shutting down camera");
        system("rosnode kill /usb_cam");
        ROS_INFO("close camera done");
        res.ret = 0;
    }
    else
    {
        ROS_INFO("starting camera");
        system("roslaunch usb_cam usb_cam.launch & ");
        ROS_INFO("camera is up");
        res.ret = 1;
    }
}

int main(int argc, char** argv)
{
    ros::init(argc,argv,"camServer");
    ros::NodeHandle nh;
    ros::ServiceServer server = nh.advertiseService("control_cam",control);
    ROS_INFO("------ waiting for client's request ------");
    ros::spin();
    return 0;
}

bool ServiceManager::advertiseService(const AdvertiseServiceOptions& ops)
{
   boost::recursive_mutex::scoped_lock shutdown_lock(shutting_down_mutex_);
   if (shutting_down_)
   {
     return false;
   }
   {
     boost::mutex::scoped_lock lock(service_publications_mutex_);
     // 如果service已经发布，就报错，然后返回，其实这里改成报警比较合适
     if (isServiceAdvertised(ops.service))
     {
       ROS_ERROR("Tried to advertise a service that is already advertised in this node [%s]", ops.service.c_str());
       return false;
     }

bool serviceCB(mow_msgs::task::Request &req, mow_msgs::task::Response &res)
{
  // ......
  res.ret = "to charge";
  // 耗时的工作
}

#include <ros/ros.h>
#include <riki_msgs/ctrl.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "controlCam");
    if(argc!=2)
    {
        ROS_INFO("client need command");
        return 1;
    }
    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient<riki_msgs::ctrl>("control_cam");
    riki_msgs::ctrl srv;
    srv.request.cmd = atoi(argv[1]);
    if(client.exists())
    {
        ROS_INFO("service control_cam is up");
        ROS_INFO("service name:%s",client.getService().c_str());
    }
    else
    {
        ROS_ERROR("service control_cam is not available");
        return 1;
    }
    if(client.call(srv))
    {
        ROS_INFO("client calling srv !");
        if(srv.request.cmd>0)
            ROS_INFO("Sending command start!");
        else
            ROS_INFO("Sending command stop!");
    }
    else
    {
        ROS_ERROR("client calls srv failed !");
        return 1;
    }
    // 调用成功以后才能获取response
    cout<<"服务端的返回值: "<<srv.response.ret<<endl;
    return 0;
}

std_srvs::Empty srv;
ros::service::call("/move_base_node/clear_costmaps", srv);

while(!ros::service::call("static_map", req, resp))
{
    ROS_WARN("Request for map failed; trying again...");
    ros::Duration d(0.5);
    d.sleep();
}

bool call (Service &service)  //调用service，client发起通信。成功返回true

bool exists ()  //检查相应名称的服务是否可用

std::string getService ()  //返回客户端通信的服务名称
// 单例模式
static const ServiceManagerPtr &  instance ()

bool    unadvertiseService (const std::string &serv_name)

make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

std::string makePlanServiceName = "/move_base/"+name+"/make_plan";

ros::ServiceManager::instance()->unadvertiseService(makePlanServiceName);
make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

Matrix3x3 &   getBasis ()    //Return the basis matrix for the rotation
const Vector3 & getOrigin ()  //Return the origin vector translation
Quaternion  getRotation ()  //Return a quaternion representing the rotation
Transform   operator* (const Transform &t) const  //Return the product of this transform and the other.
Transform   inverse ()    //Return the inverse of this transform

TransformListener::TransformListener(
    ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME),  
    bool spin_thread = true 
)

geometry_msgs::PoseStamped pose_odom;
pose_odom.header = odom->header;
pose_odom.pose = odom->pose.pose;

geometry_msgs::PoseStamped pose_map;
try{
    listener.transformPose("map", pose_odom, pose_map);
}
catch( tf::TransformException ex)
{
    ROS_WARN("transfrom exception : %s",ex.what());
    return;
}

double x=ros::Time::now().toSec();     //把时间戳转化成浮点型格式
ros::Time stamp = ros::Time().fromSec(x);    //把浮点型变成时间戳

template<class T>
T clamp(T x, T min, T max)
{
    if (x > max)
        return max;
    if (x < min)
        return min;
    return x;
}

// 注意最后一个路径点，是 end() 的前一个点
geometry_msgs::PoseStamped  global_end = *(std::prev(global_plan_.end()) );
geometry_msgs::PoseStamped  base_end;
transformPose("base_link", global_end,  base_end);

bool  transformPose(
    const std::string frame,
    geometry_msgs::PoseStamped &in_pose,
    geometry_msgs::PoseStamped &out_pose) const
{
    if (in_pose.header.frame_id == frame)
    {
        out_pose = in_pose;
        return true;
    }
    try
    {
        out_pose.header.frame_id = frame;
        tf_->transformPose(frame, in_pose, out_pose);
        out_pose.header.seq = in_pose.header.seq;
        return true;
    }
    catch (tf2::TransformException &ex)
    {
        ROS_ERROR("Exception in transformPose : %s", ex.what()  );
    }
    return false;
}

double quaternionToYaw(const geometry_msgs::Quaternion&  quat)
{
    tf::Quaternion tf_quat;
    tf::quaternionMsgToTF(quat, tf_quat);
    double roll, pitch, yaw;
    tf::Matrix3x3(tf_quat).getRPY(roll, pitch, yaw);
    return yaw;
}

double quaternionToYaw(const geometry_msgs::Quaternion&  quat)
{
	tf::Quaternion quat(0,0, quat.z, quat.w);
	return tf::getYaw(quat);
}

//只通过yaw计算四元数，用于平面小车
static geometry_msgs::Quaternion  tf::createQuaternionMsgFromYaw(double yaw);

static tf::Quaternion  createQuaternionFromYaw (double yaw)

static tf::Quaternion  createQuaternionFromRPY(double roll, double pitch, double yaw)

static geometry_msgs::Quaternion  createQuaternionMsgFromRollPitchYaw( double roll, 
    double pitch,   double yaw)

void normAngle ( double& angle )
{
    if( angle >= M_PI)
        angle -= 2 * M_PI;
    if( angle < -M_PI)
        angle += 2 * M_PI;
}

bool robotIsStill()
{
    bool velStatus;
    boost::shared_ptr<nav_msgs::Odometry const> twistEdge;
    twistEdge = ros::topic::waitForMessage<nav_msgs::Odometry>("odom",ros::Duration(0.3) );
    if(twistEdge != NULL)
    {
        double velX = twistEdge->twist.twist.linear.x;
        double angularZ = twistEdge->twist.twist.angular.z;
        if(velX == 0 && angularZ == 0)
            velStatus = true;
        else
            velStatus = false;
    }
    else velStatus = false;
    return velStatus;
}

geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = -0.12;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;

auto t1 = std::chrono::steady_clock::now();
auto t2 = std::chrono::steady_clock::now();
ROS_INFO(" -------------- backward begin ! ------------------ ");
while( std::chrono::duration<double>(t2-t1).count() < 3 )
{
    vel_pub.publish(cmd_vel);
    t2 = std::chrono::steady_clock::now();
}
cmd_vel.linear.x = 0;
vel_pub.publish(cmd_vel);
ROS_INFO(" -------------- backward done, now continue !  ------------------ ");

tf::Stamped<tf::Pose> robot_vel_tf;
base_local_planner::OdometryHelperRos odom_helper_;
odom_helper_.getRobotVel(robot_vel_tf);

geometry_msgs::Twist  robot_vel_;
robot_vel_.linear.x = robot_vel_tf.getOrigin().getX();
robot_vel_.linear.y = robot_vel_tf.getOrigin().getY();
robot_vel_.angular.z = tf::getYaw(robot_vel_tf.getRotation());

//get the starting pose of the robot
tf::Stamped<tf::Pose> global_pose;
if(!planner_costmap_ros_->getRobotPose(global_pose)) {
  ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
  return false;
}
geometry_msgs::PoseStamped start_pose;
tf::poseStampedTFToMsg(global_pose, start_pose);

tf_.transformPose(global_frame, goal_pose, global_pose);

std::vector<geometry_msgs::Point> footprint = planner_costmap_ros_->getRobotFootprint();
costmap_2d::calculateMinAndMaxDistances(footprint, robot_inscribed_radius_, robot_circumscribed_radius);
// 或者这样
planner_costmap_ros_->getLayeredCostmap()->getInscribedRadius()
planner_costmap_ros_->getLayeredCostmap()->getCircumscribedRadius()

ros::Rate rate(10);
while(ros::ok())
{
    // do something
    rate.sleep();
}

typedef std::vector<std::string> ros::V_string
// Retreives the currently-known list of nodes from the master. 放入
ROSCPP_DECL bool   getNodes (V_string &nodes)

std::vector<std::string> nodes;
ros::master::getNodes(nodes);	// bool
int len = nodes.size();
for (int i =0; i < len; i ++)
{
	cout<<"node: "<<nodes.at(i)<<endl;
}

struct ros::master::TopicInfo
{
	TopicInfo ()
 	TopicInfo (const std::string &_name, const std::string &_datatype)
public:
	std::string 	datatype    // Datatype of the topic. 
	std::string 	name        // Name of the topic. 
}
typedef std::vector<TopicInfo> ros::master::V_TopicInfo

ROSCPP_DECL bool    getTopics (V_TopicInfo &topics)

std::vector<ros::master::TopicInfo> topics;
ros::master::getTopics(topics);
int len = topics.size();
for (int i =0; i < len; i ++)
{
	cout<<"topic: "<<topics.at(i).name<<"   type: "
		<<topics.at(i).datatype<<endl;
}

ros::isInitialized ()    // 判断ros::init()是否被调用

ros::isStarted ()	　// 判断节点是否已经通过ros::start()启动

ros::requestShutdown ()　　// 在节点的一个线程内要求ROS关闭

ros::this_node::getName       // 返回当前节点的名称

getAdvertisedTopics()　　//返回节点注册的话题
getSubscribedTopics()   //返回节点订阅的主题

getNamespace()	//返回节点的命名空间

ROS_ASSERT(a<b)			//同assert()，若条件不满足，程序会终止

ROS_BREAK()				//直接终止程序，报警FATAL


 // Get the hostname where the master runs. 
ROSCPP_DECL const std::string &     getHost ()

// Get the port where the master runs. 
ROSCPP_DECL uint32_t    getPort ()

// Get the full URI to the master (eg. http://host:port/) 
ROSCPP_DECL const std::string &     getURI ()

while(ros::ok())
{
    // Do Something
    ros::spinOnce();
}

//发送频率为10Hz(1秒发10次)  消息池最大容量1000。
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
ros::Rate loop_rate(10);
while (ros::ok())
{
	// 发布消息
}


ros::Subscriber sub = n.subscribe("chatter", 2, chatterCallback);
ros::Rate loop_rate(5);
while (ros::ok())
{
    /*...TODO...*/ 
    ros::spinOnce();
    loop_rate.sleep();
}

void spinOnce()
{
  g_global_queue->callAvailable(ros::WallDuration());
}