<|start_filename|>robot/src/navi/src/navi_adapter.cpp<|end_filename|>
// @author <NAME>

#include <iostream>

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "basic/moveMsg.h"

using namespace std;
double LINEAR_VEL_MAX = 0.5;
double ANGULAR_VEL_MAX = 1.0;

basic::moveMsg new_move_msg;
ros::Publisher vel_pub;

double set_new_vel(double cur_vel, bool is_ang)
{
    double vel_lim = is_ang ? ANGULAR_VEL_MAX : LINEAR_VEL_MAX;
    if (cur_vel < 0)
    {
        return max(cur_vel, -vel_lim);
    }
    else if (cur_vel > 0)
    {
        return min(cur_vel, vel_lim);
    }
    return 0;
}

void adapt(const geometry_msgs::Twist::ConstPtr &vel)
{
    new_move_msg.x = set_new_vel(vel->linear.x, false);
    new_move_msg.y = set_new_vel(vel->linear.y, false);
    new_move_msg.z = set_new_vel(vel->angular.z, true);
    vel_pub.publish(new_move_msg);
}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "navi_adapter");
    ros::NodeHandle n;

    vel_pub = n.advertise<basic::moveMsg>("/fight_with_hair/basic_move/vel", 10);
    ros::Subscriber sub_trans = n.subscribe("/fight_with_hair/robot_navi/vel", 10, adapt);

    ros::spin();

    return 0;
}

<|start_filename|>robot/src/drivers/waterplus_map_tools/include/UDPServer.h<|end_filename|>
#ifndef _EXS_UDPSERVER_H
#define _EXS_UDPSERVER_H
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>

#define bool char
#define true 1
#define false 0

#define CTRL_STOP           0x00
#define CTRL_MOVETO_POS     0x01  
#define CTRL_MOVETO_NAME    0x02
#define CTRL_GRAB           0x03
#define CTRL_PASS           0x04

typedef struct ST_Ctrl
{
    int ctrl;
    float x;
    float y;
    float angle;
    char wp_name;
}ST_Ctrl;

void InitUDPServer(int inPort);
int UDPServerLoop();
bool GetCtrlCmd(ST_Ctrl* inCtrl);
#endif


<|start_filename|>robot/src/drivers/wpb_home/wpb_home_behaviors/src/wpb_home_face_detect_3d.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*!******************************************************************
 @author     ZhangWanjie
 ********************************************************************/

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/objdetect/objdetect.hpp"
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <iostream>
#include <wpb_home_behaviors/Rect.h>
#include <wpb_home_behaviors/Coord.h>

using namespace std;
using namespace cv;

static std::string rgb_topic;
static std::string pc_topic;
static std::string face_cascade_name;
static CascadeClassifier face_cascade;

static const std::string WIN_TITLE = "face";
static Mat frame_gray;
static ros::Publisher image_pub;
static std::vector<Rect> faces;
static std::vector<cv::Rect>::const_iterator face_iter;


static ros::Publisher rect_pub;
static ros::Publisher coord_pub;
static ros::Publisher pc_pub;
static tf::TransformListener *tf_listener; 
static ros::Publisher marker_pub;
static visualization_msgs::Marker line_face;
static visualization_msgs::Marker pos_face;
static visualization_msgs::Marker text_marker;

cv::Mat drawFacesRGB(cv::Mat inImage) 
{
    for (face_iter = faces.begin(); face_iter != faces.end(); ++face_iter) 
    {
        cv::rectangle(
            inImage,
            cv::Point(face_iter->x , face_iter->y),
            cv::Point(face_iter->x + face_iter->width, face_iter->y + face_iter->height),
            CV_RGB(255, 0 , 255),
            10);
    }
    return inImage;
}

void PublishFaceRect()
{
    int face_index = 0;
    wpb_home_behaviors::Rect rect;
    for (face_iter = faces.begin(); face_iter != faces.end(); ++face_iter) 
    {
        int top = face_iter->y;
        int bottom = face_iter->y+face_iter->height;
        int left = face_iter->x;
        int right = face_iter->x + face_iter->width;
        rect.top.push_back(top);
        rect.bottom.push_back(bottom);
        rect.left.push_back(left);
        rect.right.push_back(right);
        rect.probability.push_back(1.0f);
        std::ostringstream stringStream;
        stringStream << "Face_" << face_index;
        std::string face_id = stringStream.str();
        rect.name.push_back(face_id);
        face_index ++;
    }
    rect_pub.publish(rect);
}

void DrawTextMarker(std::string inText, int inID, float inScale, float inX, float inY, float inZ, float inR, float inG, float inB)
{
    text_marker.header.frame_id = "base_footprint";
    text_marker.ns = "line_obj";
    text_marker.action = visualization_msgs::Marker::ADD;
    text_marker.id = inID;
    text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    text_marker.scale.z = inScale;
    text_marker.color.r = inR;
    text_marker.color.g = inG;
    text_marker.color.b = inB;
    text_marker.color.a = 1.0;

    text_marker.pose.position.x = inX;
    text_marker.pose.position.y = inY;
    text_marker.pose.position.z = inZ;
    
    text_marker.pose.orientation=tf::createQuaternionMsgFromYaw(1.0);

    text_marker.text = inText;

    marker_pub.publish(text_marker);
}

void callbackRGB(const sensor_msgs::ImageConstPtr& msg)
{
    //ROS_INFO("callbackRGB");
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    // change contrast: 0.5 = half  ; 2.0 = double
    cv_ptr->image.convertTo(frame_gray, -1, 1.5, 0);

    // create B&W image
    cvtColor( frame_gray, frame_gray, CV_BGR2GRAY );

	equalizeHist( frame_gray, frame_gray );
    //-- Detect faces
	face_cascade.detectMultiScale( frame_gray, faces, 1.1, 9, 0|CASCADE_SCALE_IMAGE, Size(30, 30) );

    //ROS_INFO("face = %d",faces.size());
    if(faces.size() > 0)
    {
        cv_ptr->image = drawFacesRGB(cv_ptr->image);
    }
    PublishFaceRect();

    image_pub.publish(cv_ptr->toImageMsg());
}

void callbackPointCloud(const sensor_msgs::PointCloud2 &input)
{
     //to footprint
    sensor_msgs::PointCloud2 pc_footprint;
    bool res = tf_listener->waitForTransform("/base_footprint", input.header.frame_id, input.header.stamp, ros::Duration(5.0)); 
    if(res == false)
    {
        return;
    }
    pcl_ros::transformPointCloud("/base_footprint", input, pc_footprint, *tf_listener);

    //source cloud
    pcl::PointCloud<pcl::PointXYZRGB> cloud_src;
    pcl::fromROSMsg(pc_footprint , cloud_src);
    //ROS_WARN("cloud_src size = %d  width = %d",cloud_src.size(),input.width); 

    ////////////////////////////////
    // Draw Face Boxes
    line_face.points.clear();
    line_face.header.frame_id = "base_footprint";
    line_face.ns = "line_face";
    line_face.action = visualization_msgs::Marker::ADD;
    line_face.id = 1;
    line_face.type = visualization_msgs::Marker::LINE_LIST;
    line_face.scale.x = 0.01;
    line_face.color.r = 1.0;
    line_face.color.g = 0;
    line_face.color.b = 1.0;
    line_face.color.a = 1.0;

    pos_face.points.clear();
    pos_face.header.frame_id = "base_footprint";
    pos_face.ns = "pos_face";
    pos_face.action = visualization_msgs::Marker::ADD;
    pos_face.id = 1;
    pos_face.type = visualization_msgs::Marker::CUBE_LIST;
    pos_face.scale.x = 0.5;
    pos_face.scale.y = 0.5;
    pos_face.scale.z = 0.001;
    pos_face.color.r = 1.0;
    pos_face.color.g = 0;
    pos_face.color.b = 1.0;
    pos_face.color.a = 1.0;

    wpb_home_behaviors::Coord coord;
    geometry_msgs::Point p;
    int nFaceIndex = 0;
    std::vector<cv::Rect>::const_iterator i;
    for (face_iter = faces.begin(); face_iter != faces.end(); ++face_iter) 
    {
        int rgb_face_x = face_iter->x  + face_iter->width/2;
        int rgb_face_y = face_iter->y + face_iter->height/2;
        int index_pc = rgb_face_y*input.width + rgb_face_x;
        float face_x = cloud_src.points[index_pc].x;
        float face_y = cloud_src.points[index_pc].y;
        float face_z = cloud_src.points[index_pc].z;
       
        p.x = 0.2; p.y = 0; p.z = 1.37; line_face.points.push_back(p);
        //p.x = -0.1; p.y = 0; p.z = 1.25; line_face.points.push_back(p);
        p.x = face_x; p.y = face_y; p.z = face_z; line_face.points.push_back(p);
        p.z = 0;pos_face.points.push_back(p);

        std::ostringstream stringStream;
        stringStream << "Face_" << nFaceIndex;
        std::string face_id = stringStream.str();
        DrawTextMarker(face_id,nFaceIndex,0.1,face_x,face_y,face_z+0.2,0,1.0,0);
        
        coord.name.push_back(face_id);
        coord.x.push_back(face_x);
        coord.y.push_back(face_y);
        coord.z.push_back(face_z);
        coord.probability.push_back(1.0f);

        ROS_WARN("face_%d (%d,%d) - (%.2f %.2f %.2f)",nFaceIndex,rgb_face_x,rgb_face_y,face_x,face_y,face_z); 

        nFaceIndex ++;
    }
    marker_pub.publish(line_face);
    marker_pub.publish(pos_face);
    coord_pub.publish(coord);

    for(int y=0; y< 300; y++)
    {
        for(int x=0; x< 200; x++)
        {
            int index_pc = y*input.width + x;
            cloud_src.points[index_pc].r = 1.0f;
            cloud_src.points[index_pc].g = 0.0f;
            cloud_src.points[index_pc].b = 1.0f;
        }
    }
    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(cloud_src, output);

    output.header.frame_id = pc_footprint.header.frame_id;
    pc_pub.publish(output);
}

int main(int argc, char **argv)
{
    cv::namedWindow(WIN_TITLE);
    ros::init(argc, argv, "wpb_home_face_detect_3d");
    ros::NodeHandle nh_param("~");
    //nh_param.param<std::string>("rgb_topic", rgb_topic, "/camera/image_raw");
    nh_param.param<std::string>("rgb_topic", rgb_topic, "/kinect2/hd/image_color");
    nh_param.param<std::string>("topic", pc_topic, "/kinect2/hd/points");
    nh_param.param<std::string>("face_cascade_name", face_cascade_name, "haarcascade_frontalface_alt.xml");

    ROS_WARN("wpb_home_face_detect_3d start!");

    bool res = face_cascade.load(face_cascade_name);
	if (res == false)
	{
		ROS_ERROR("fail to load haarcascade_frontalface_alt.xml");
        return 0;
	}

    tf_listener = new tf::TransformListener(); 
    ros::NodeHandle nh;
    ros::Subscriber rgb_sub = nh.subscribe(rgb_topic, 1 , callbackRGB);
    ros::Subscriber pc_sub = nh.subscribe(pc_topic, 1 , callbackPointCloud);

    image_pub = nh.advertise<sensor_msgs::Image>("/face/image", 2);
    marker_pub = nh.advertise<visualization_msgs::Marker>("face_marker", 2);
    pc_pub = nh.advertise<sensor_msgs::PointCloud2>("face_pointcloud",1);
    rect_pub = nh.advertise<wpb_home_behaviors::Rect>("/wpb_home/face_detect_2d", 10);
    coord_pub = nh.advertise<wpb_home_behaviors::Coord>("/wpb_home/face_detect_3d", 10);

    ros::Rate loop_rate(30);
    while( ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }

    cv::destroyWindow(WIN_TITLE);
    delete tf_listener; 

    return 0;
}

<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/main_window.hpp<|end_filename|>
/**
 * @file /include/fight_with_hair_ui/main_window.hpp
 *
 * @brief Qt based gui for fight_with_hair_ui.
 *
 * @date November 2010
 **/
#ifndef fight_with_hair_ui_MAIN_WINDOW_H
#define fight_with_hair_ui_MAIN_WINDOW_H

/*****************************************************************************
** Includes
*****************************************************************************/

#include <QtWidgets/QMainWindow>
#include "ui_main_window.h"
#include "qnode.hpp"
#include <string>

/*****************************************************************************
** Namespace
*****************************************************************************/

namespace fight_with_hair_ui {

/*****************************************************************************
** Interface [MainWindow]
*****************************************************************************/
/**
 * @brief Qt central, all operations relating to the view part here.
 */
class MainWindow : public QMainWindow {
  Q_OBJECT

public:
  MainWindow(int argc, char** argv, QWidget *parent = 0);
  ~MainWindow();
  void ReadSettings(); // Load up qt program settings at startup
  void WriteSettings(); // Save qt program settings when closing

  void closeEvent(QCloseEvent *event); // Overloaded function

  void chooseEnvironment();

public Q_SLOTS:
  void receive_asr_orders(std::string);
  /******************************************
  ** Auto-connections (connectSlotsByName())
  *******************************************/


private slots:
  void on_control_with_key_clicked();

  void on_navigation_clicked();

  void on_build_map_clicked();

  void on_detection_clicked();

  void on_logout_clicked();

  void on_follow_clicked();

  void on_ASR_clicked();

  void on_aaaaa_clicked();

  void on_autoMap_clicked();

Q_SIGNALS:
  void send_follow_end_msg();

private:
  Ui::MainWindowDesign ui;
  QNode qnode;
  bool isSim=false;
};

}  // namespace fight_with_hair_ui

#endif // fight_with_hair_ui_MAIN_WINDOW_H


<|start_filename|>robot/src/drivers/xfyun_waterplus/src/iat_file.cpp<|end_filename|>
/* Copiright belong to xfyun(IFLYTEK CO.,LTD)                            */
/* @modified by <NAME>                                             */

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include "qisr.h"
#include "msp_cmn.h"
#include "msp_errors.h"
#include "speech_recognizer.h"
#include <iconv.h>
#include <ros/ros.h>
#include <std_msgs/String.h>

#define FRAME_LEN	640 
#define	BUFFER_SIZE	4096

static ros::Publisher iat_pub;
static char session_begin_params[512];
static bool bCN = false;
static bool bActive = false;

static void show_result(char *string, char is_over)
{
	if(bCN == false)
	{
		ROS_WARN("[%s]",string);
	}
	else
	{
		printf("\r识别结果:[ %s ]\n",  string);
	}

    if(is_over)
		putchar('\n');
}

static char *g_result = NULL;
static unsigned int g_buffersize = BUFFER_SIZE;

void on_result(const char *result, char is_last)
{
	if (result) {
		size_t left = g_buffersize - 1 - strlen(g_result);
		size_t size = strlen(result);
		if (left < size) {
			g_result = (char*)realloc(g_result, g_buffersize + BUFFER_SIZE);
			if (g_result)
				g_buffersize += BUFFER_SIZE;
			else {
				printf("mem alloc failed\n");
				return;
			}
		}
		strncat(g_result, result, size);
		show_result(g_result, is_last);

		std_msgs::String strResult;
		std::string str(result);
		strResult.data = str;
		iat_pub.publish(strResult);
	}
}
void on_speech_begin()
{
	if (g_result)
	{
		free(g_result);
	}
	g_result = (char*)malloc(BUFFER_SIZE);
	g_buffersize = BUFFER_SIZE;
	memset(g_result, 0, g_buffersize);

	//printf("Start Listening...\n");
	printf("开始语音识别...\n");
}
void on_speech_end(int reason)
{
	if (reason == END_REASON_VAD_DETECT)
		//printf("\nSpeaking done \n");
		printf("正在准备下一次语音识别,请稍等... \n");
	else
		printf("\nRecognizer error %d\n", reason);
}

/* demo send audio data from a file */
static void demo_file(const char* audio_file, const char* session_begin_params)
{
	int	errcode = 0;
	FILE*	f_pcm = NULL;
	char*	p_pcm = NULL;
	unsigned long	pcm_count = 0;
	unsigned long	pcm_size = 0;
	unsigned long	read_size = 0;
	struct speech_rec iat;
	struct speech_rec_notifier recnotifier = {
		on_result,
		on_speech_begin,
		on_speech_end
	};

	if (NULL == audio_file)
		goto iat_exit;

	f_pcm = fopen(audio_file, "rb");
	if (NULL == f_pcm)
	{
		printf("\nopen [%s] failed! \n", audio_file);
		goto iat_exit;
	}

	fseek(f_pcm, 0, SEEK_END);
	pcm_size = ftell(f_pcm);
	fseek(f_pcm, 0, SEEK_SET);

	p_pcm = (char *)malloc(pcm_size);
	if (NULL == p_pcm)
	{
		printf("\nout of memory! \n");
		goto iat_exit;
	}

	read_size = fread((void *)p_pcm, 1, pcm_size, f_pcm);
	if (read_size != pcm_size)
	{
		printf("\nread [%s] error!\n", audio_file);
		goto iat_exit;
	}

	errcode = sr_init(&iat, session_begin_params, SR_USER, &recnotifier);
	if (errcode) {
		printf("speech recognizer init failed : %d\n", errcode);
		goto iat_exit;
	}

	errcode = sr_start_listening(&iat);
	if (errcode) {
		printf("\nsr_start_listening failed! error code:%d\n", errcode);
		goto iat_exit;
	}

	while (1)
	{
		unsigned int len = 10 * FRAME_LEN; /* 200ms audio */
		int ret = 0;

		if (pcm_size < 2 * len)
			len = pcm_size;
		if (len <= 0)
			break;

		ret = sr_write_audio_data(&iat, &p_pcm[pcm_count], len);

		if (0 != ret)
		{
			printf("\nwrite audio data failed! error code:%d\n", ret);
			goto iat_exit;
		}

		pcm_count += (long)len;
		pcm_size -= (long)len;		
	}

	errcode = sr_stop_listening(&iat);
	if (errcode) {
		printf("\nsr_stop_listening failed! error code:%d \n", errcode);
		goto iat_exit;
	}

iat_exit:
	if (NULL != f_pcm)
	{
		fclose(f_pcm);
		f_pcm = NULL;
	}
	if (NULL != p_pcm)
	{
		free(p_pcm);
		p_pcm = NULL;
	}

	sr_stop_listening(&iat);
	sr_uninit(&iat);
}

void rec_file_callback(const std_msgs::String::ConstPtr& msg)
{
    const char* audio_file = msg->data.c_str();
	
	demo_file(audio_file,session_begin_params);
}

int main(int argc, char* argv[])
{

	ros::init(argc, argv, "xfyun_iat_record");

    ros::NodeHandle n;
    iat_pub = n.advertise<std_msgs::String>("/xfyun/iat", 10);
	ros::NodeHandle n_param("~");
    n_param.param<bool>("cn", bCN, false); 
    n_param.param<bool>("start", bActive, true);

	if(bCN == true)
		strcpy(session_begin_params,"sub = iat, ptt = 0, domain = iat, language = zh_cn, \
		accent = mandarin, sample_rate = 16000, \
		result_type = plain, result_encoding = utf8");
	else
		strcpy(session_begin_params,"sub = iat, ptt = 0, domain = iat, language = en_us, \
		accent = mandarin, sample_rate = 16000, \
		result_type = plain, result_encoding = utf8");

	int ret = MSP_SUCCESS;
	const char* login_params = "appid = 58eeeedf, work_dir = .";
	ret = MSPLogin(NULL, NULL, login_params);
	if (MSP_SUCCESS != ret)	{
		printf("MSPLogin failed , Error code %d.\n",ret);
		MSPLogout();
		return 0;
	}

	ros::Subscriber sub =n.subscribe("/xfyun/reco_file",2,rec_file_callback);
	//sleep(1);
	//demo_file("/home/robot/s0.wav",session_begin_params);	//test
	ros::spin();

	MSPLogout(); // Logout...

	return 0;
}


<|start_filename|>robot/src/drivers/waterplus_map_tools/src/wp_manager.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* @author <NAME>                                             */

#include <tinyxml.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <waterplus_map_tools/Waypoint.h>
#include <waterplus_map_tools/GetNumOfWaypoints.h>
#include <waterplus_map_tools/GetWaypointByIndex.h>
#include <waterplus_map_tools/GetWaypointByName.h>
#include <waterplus_map_tools/SaveWaypoints.h>
#include <string>

static std::vector <waterplus_map_tools::Waypoint> arWaypoint;
static std::vector <waterplus_map_tools::Waypoint> arCharger;
static ros::Publisher marker_pub;
static ros::Publisher charger_pub;
static visualization_msgs::Marker marker_waypoints;
static visualization_msgs::Marker marker_chargers;
static visualization_msgs::Marker text_marker;

bool getNumOfWaypoints(waterplus_map_tools::GetNumOfWaypoints::Request &req, waterplus_map_tools::GetNumOfWaypoints::Response &res)
{
    res.num = arWaypoint.size();
    ROS_INFO("Get_num_wp: num_wp = %d", res.num);
    return true;
}

bool getWaypointByIndex(waterplus_map_tools::GetWaypointByIndex::Request &req, waterplus_map_tools::GetWaypointByIndex::Response &res)
{
    int nIndex = req.index;
    int nNumWP = arWaypoint.size();
    if(nIndex >= 0 && nIndex < nNumWP)
    {
        res.name = arWaypoint[nIndex].name;
        res.pose = arWaypoint[nIndex].pose;
        ROS_INFO("Get_wp_index: name = %s", arWaypoint[nIndex].name.c_str());
        return true;
    }
    else
    {
        ROS_INFO("Get_wp_index: failed! index = %d , num_wp = %d", nIndex , nNumWP);
        return false;
    }
}

bool getWaypointByName(waterplus_map_tools::GetWaypointByName::Request &req, waterplus_map_tools::GetWaypointByName::Response &res)
{
    std::string reqName = req.name;
    int nNumWP = arWaypoint.size();
    bool bResultGetWP = false;
    for(int i=0;i<nNumWP;i++)
    {
        std::size_t found = arWaypoint[i].name.find(reqName); 
        if(found != std::string::npos)
        {
            res.name = arWaypoint[i].name;
            res.pose = arWaypoint[i].pose;
            bResultGetWP = true;
            break;
        }
    }
    if(bResultGetWP == true)
    {
        ROS_INFO("Get_wp_name: name = %s", res.name.c_str());
        return true;
    }
    else
    {
        ROS_INFO("Get_wp_name: failed! There is no waypoint name %s", reqName.c_str());
        return false;
    }
}

bool getChargerByName(waterplus_map_tools::GetWaypointByName::Request &req, waterplus_map_tools::GetWaypointByName::Response &res)
{
    std::string reqName = req.name;
    int nNumCh = arCharger.size();
    bool bResultGetCh = false;
    for(int i=0;i<nNumCh;i++)
    {
        std::size_t found = arCharger[i].name.find(reqName); 
        if(found != std::string::npos)
        {
            res.name = arCharger[i].name;
            res.pose = arCharger[i].pose;
            bResultGetCh = true;
            break;
        }
    }
    if(bResultGetCh == true)
    {
        ROS_INFO("Get_charger_name: name = %s", res.name.c_str());
        return true;
    }
    else
    {
        ROS_INFO("Get_charger_name: failed! There is no charger name %s", reqName.c_str());
        return false;
    }
}

bool SaveWaypointsToFile(std::string inFilename);
bool saveWaypoints(waterplus_map_tools::SaveWaypoints::Request &req, waterplus_map_tools::SaveWaypoints::Response &res)
{
    return SaveWaypointsToFile(req.filename);
}

std::string Flt2Str(float inVal)
{
    std::ostringstream stringStream;
    stringStream << inVal;
    std::string retStr = stringStream.str();
    return retStr;
}

std::string Int2Str(int inVal)
{
    std::ostringstream stringStream;
    stringStream << inVal;
    std::string retStr = stringStream.str();
    return retStr;
}

bool SaveWaypointsToFile(std::string inFilename)
{
    TiXmlDocument *docSave = new TiXmlDocument();
    TiXmlElement *RootElement = new TiXmlElement("Waterplus");
    docSave->LinkEndChild(RootElement);

    int nNumWP = arWaypoint.size();
    for(int i=0;i<nNumWP;i++)
    {
        TiXmlElement *WaypointElement = new TiXmlElement("Waypoint");
        WaypointElement->InsertEndChild(TiXmlElement("Name"))->InsertEndChild(TiXmlText(arWaypoint[i].name));
        WaypointElement->InsertEndChild(TiXmlElement("Pos_x"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.position.x)));
        WaypointElement->InsertEndChild(TiXmlElement("Pos_y"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.position.y)));
        WaypointElement->InsertEndChild(TiXmlElement("Pos_z"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.position.z)));
        WaypointElement->InsertEndChild(TiXmlElement("Ori_x"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.x)));
        WaypointElement->InsertEndChild(TiXmlElement("Ori_y"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.y)));
        WaypointElement->InsertEndChild(TiXmlElement("Ori_z"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.z)));
        WaypointElement->InsertEndChild(TiXmlElement("Ori_w"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.w)));
        RootElement->InsertEndChild(*WaypointElement);  
    }

    int nNumCharger = arCharger.size();
    for(int i=0;i<nNumCharger;i++)
    {
        TiXmlElement *ChargerElement = new TiXmlElement("Charger");
        ChargerElement->InsertEndChild(TiXmlElement("Name"))->InsertEndChild(TiXmlText(arCharger[i].name));
        ChargerElement->InsertEndChild(TiXmlElement("Pos_x"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.position.x)));
        ChargerElement->InsertEndChild(TiXmlElement("Pos_y"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.position.y)));
        ChargerElement->InsertEndChild(TiXmlElement("Pos_z"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.position.z)));
        ChargerElement->InsertEndChild(TiXmlElement("Ori_x"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.x)));
        ChargerElement->InsertEndChild(TiXmlElement("Ori_y"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.y)));
        ChargerElement->InsertEndChild(TiXmlElement("Ori_z"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.z)));
        ChargerElement->InsertEndChild(TiXmlElement("Ori_w"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.w)));
        RootElement->InsertEndChild(*ChargerElement);  
    }

    bool res = docSave->SaveFile(inFilename);
    if(res == true)
        ROS_INFO("Saved waypoints to file! filename = %s", inFilename.c_str());
    else
        ROS_INFO("Failed to save waypoints... filename = %s", inFilename.c_str());

    return res;
}

bool LoadWaypointsFromFile(std::string inFilename)
{
    TiXmlDocument docLoad(inFilename);
    bool resLoad = docLoad.LoadFile();
    if(resLoad == false)
    {
        ROS_INFO("Failed to load waypoints... filename = %s", inFilename.c_str());
        return false;
    }

    waterplus_map_tools::Waypoint newWayPoint;
    TiXmlElement* RootElement = docLoad.RootElement();
    for(TiXmlNode* item = RootElement->FirstChild("Waypoint");item;item = item->NextSibling("Waypoint"))
    {
        TiXmlNode* child = item->FirstChild();
        const char* name = child->ToElement()->GetText();
        ROS_INFO("Load waypoint : %s", name);
        newWayPoint.name = std::string(name); 
        child = item->IterateChildren(child);
        const char* pos_x = child->ToElement()->GetText();
        newWayPoint.pose.position.x = std::atof(pos_x);
        child = item->IterateChildren(child);
        const char* pos_y = child->ToElement()->GetText();
        newWayPoint.pose.position.y = std::atof(pos_y);
        child = item->IterateChildren(child);
        const char* pos_z = child->ToElement()->GetText();
        newWayPoint.pose.position.z = std::atof(pos_z);
        child = item->IterateChildren(child);
        const char* ori_x = child->ToElement()->GetText();
        newWayPoint.pose.orientation.x = std::atof(ori_x);
        child = item->IterateChildren(child);
        const char* ori_y = child->ToElement()->GetText();
        newWayPoint.pose.orientation.y = std::atof(ori_y);
        child = item->IterateChildren(child);
        const char* ori_z = child->ToElement()->GetText();
        newWayPoint.pose.orientation.z = std::atof(ori_z);
        child = item->IterateChildren(child);
        const char* ori_w = child->ToElement()->GetText();
        newWayPoint.pose.orientation.w = std::atof(ori_w);
        arWaypoint.push_back(newWayPoint);
    }

    for(TiXmlNode* item = RootElement->FirstChild("Charger");item;item = item->NextSibling("Charger"))
    {
        TiXmlNode* child = item->FirstChild();
        const char* name = child->ToElement()->GetText();
        ROS_INFO("Load charger : %s", name);
        newWayPoint.name = std::string(name); 
        child = item->IterateChildren(child);
        const char* pos_x = child->ToElement()->GetText();
        newWayPoint.pose.position.x = std::atof(pos_x);
        child = item->IterateChildren(child);
        const char* pos_y = child->ToElement()->GetText();
        newWayPoint.pose.position.y = std::atof(pos_y);
        child = item->IterateChildren(child);
        const char* pos_z = child->ToElement()->GetText();
        newWayPoint.pose.position.z = std::atof(pos_z);
        child = item->IterateChildren(child);
        const char* ori_x = child->ToElement()->GetText();
        newWayPoint.pose.orientation.x = std::atof(ori_x);
        child = item->IterateChildren(child);
        const char* ori_y = child->ToElement()->GetText();
        newWayPoint.pose.orientation.y = std::atof(ori_y);
        child = item->IterateChildren(child);
        const char* ori_z = child->ToElement()->GetText();
        newWayPoint.pose.orientation.z = std::atof(ori_z);
        child = item->IterateChildren(child);
        const char* ori_w = child->ToElement()->GetText();
        newWayPoint.pose.orientation.w = std::atof(ori_w);
        arCharger.push_back(newWayPoint);
    }

    return true;
}

void Init_Marker()
{
    marker_waypoints.header.frame_id = "map";
    marker_waypoints.ns = "marker_waypoints";
    marker_waypoints.action = visualization_msgs::Marker::ADD;
    marker_waypoints.type = visualization_msgs::Marker::MESH_RESOURCE;
    marker_waypoints.mesh_resource = "package://waterplus_map_tools/meshes/waypoint.dae";
    marker_waypoints.scale.x = 1;
    marker_waypoints.scale.y = 1;
    marker_waypoints.scale.z = 1;
    marker_waypoints.color.r = 1.0;
    marker_waypoints.color.g = 0.0;
    marker_waypoints.color.b = 1.0;
    marker_waypoints.color.a = 1.0;

    marker_chargers.header.frame_id = "map";
    marker_chargers.ns = "marker_waypoints";
    marker_chargers.action = visualization_msgs::Marker::ADD;
    marker_chargers.type = visualization_msgs::Marker::MESH_RESOURCE;
    marker_chargers.mesh_resource = "package://waterplus_map_tools/meshes/charger.dae";
    marker_chargers.scale.x = 1;
    marker_chargers.scale.y = 1;
    marker_chargers.scale.z = 1;
    marker_chargers.color.r = 0.5;
    marker_chargers.color.g = 0.0;
    marker_chargers.color.b = 1.0;
    marker_chargers.color.a = 1.0;
}

void DrawTextMarker(ros::Publisher* inPub, std::string inText, int inID, float inScale, float inX, float inY, float inZ, float inR, float inG, float inB);
void PublishWaypointsMarker()
{
    int nNumWP = arWaypoint.size();
    for(int i=0; i<nNumWP ; i++ )
    {
        marker_waypoints.id = i;
        marker_waypoints.pose.position.x = arWaypoint[i].pose.position.x;
        marker_waypoints.pose.position.y = arWaypoint[i].pose.position.y;
        marker_waypoints.pose.position.z = -0.01;
        marker_waypoints.pose.orientation = arWaypoint[i].pose.orientation;
        marker_pub.publish(marker_waypoints);
        ros::spinOnce();

        float wp_x = arWaypoint[i].pose.position.x;
        float wp_y = arWaypoint[i].pose.position.y;

        std::string wp_name = arWaypoint[i].name;
        DrawTextMarker(&marker_pub,wp_name,i,0.2,wp_x,wp_y,0.55,1.0,0.0,0.0);
        ros::spinOnce();
    }

    int nNumCh = arCharger.size();
    for(int i=0; i<nNumCh ; i++ )
    {
        marker_chargers.id = i;
        marker_chargers.pose.position.x = arCharger[i].pose.position.x;
        marker_chargers.pose.position.y = arCharger[i].pose.position.y;
        marker_chargers.pose.position.z = -0.01;
        marker_chargers.pose.orientation = arCharger[i].pose.orientation;
        
        charger_pub.publish(marker_chargers);
        ros::spinOnce();

        float ch_x = arCharger[i].pose.position.x;
        float ch_y = arCharger[i].pose.position.y;

        std::string wp_name = arCharger[i].name;
        DrawTextMarker(&charger_pub, wp_name,i,0.2,ch_x,ch_y,0.35,1.0,0.0,0.0);
        ros::spinOnce();
    }
}

void DrawTextMarker(ros::Publisher* inPub, std::string inText, int inID, float inScale, float inX, float inY, float inZ, float inR, float inG, float inB)
{
    text_marker.header.frame_id = "map";
    text_marker.ns = "text";
    text_marker.action = visualization_msgs::Marker::ADD;
    text_marker.id = inID;
    text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    text_marker.scale.z = inScale;
    text_marker.color.r = inR;
    text_marker.color.g = inG;
    text_marker.color.b = inB;
    text_marker.color.a = 1.0;

    text_marker.pose.position.x = inX;
    text_marker.pose.position.y = inY;
    text_marker.pose.position.z = inZ;
    
    text_marker.pose.orientation=tf::createQuaternionMsgFromYaw(1.0);

    text_marker.text = inText;

    inPub->publish(text_marker);
}

void AddWayPointCallback(const waterplus_map_tools::Waypoint::ConstPtr& wp)
{
    ROS_INFO("Add_waypoint: %s (%.2f %.2f) (%.2f %.2f %.2f %.2f) ",wp->name.c_str(), wp->pose.position.x, wp->pose.position.y, wp->pose.orientation.x, wp->pose.orientation.y, wp->pose.orientation.z, wp->pose.orientation.w);
    waterplus_map_tools::Waypoint newWayPoint;
    newWayPoint = *wp;
    arWaypoint.push_back(newWayPoint);
}

void AddChargerCallback(const waterplus_map_tools::Waypoint::ConstPtr& wp)
{
    ROS_INFO("Add_charger: %s (%.2f %.2f) (%.2f %.2f %.2f %.2f) ",wp->name.c_str(), wp->pose.position.x, wp->pose.position.y, wp->pose.orientation.x, wp->pose.orientation.y, wp->pose.orientation.z, wp->pose.orientation.w);
    waterplus_map_tools::Waypoint newCharger;
    newCharger = *wp;
    arCharger.push_back(newCharger);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "wp_waypoint_manager");

    std::string strLoadFile;
    char const* home = getenv("HOME");
    strLoadFile = home;
    strLoadFile += "/waypoints.xml";

    ros::NodeHandle n_param("~");
    std::string strParamFile;
    n_param.param<std::string>("load", strParamFile, "");
    if(strParamFile.length() > 0)
    {
        strLoadFile = strParamFile;
    }

    if(strLoadFile.length() > 0)
    {
        ROS_INFO("Load waypoints from file : %s",strLoadFile.c_str());
        LoadWaypointsFromFile(strLoadFile);
    }
    else
    {
        ROS_WARN("strLoadFile is empty. Failed to load waypoints!");
    }
    

    ros::NodeHandle nh;
    marker_pub = nh.advertise<visualization_msgs::Marker>("waypoints_marker", 100);
    charger_pub = nh.advertise<visualization_msgs::Marker>("chargers_marker", 100);
    Init_Marker();
    ros::Subscriber add_waypoint_sub = nh.subscribe("/waterplus/add_waypoint",10,&AddWayPointCallback);
    ros::Subscriber add_charger_sub = nh.subscribe("/waterplus/add_charger",10,&AddChargerCallback);

    ros::ServiceServer srvGetNum = nh.advertiseService("/waterplus/get_num_waypoint", getNumOfWaypoints);
    ros::ServiceServer srvGetWPIndex = nh.advertiseService("/waterplus/get_waypoint_index", getWaypointByIndex);
    ros::ServiceServer srvGetWPName = nh.advertiseService("/waterplus/get_waypoint_name", getWaypointByName);
    ros::ServiceServer srvSaveWP = nh.advertiseService("/waterplus/save_waypoints", saveWaypoints);

    ros::ServiceServer srvGetChargerName = nh.advertiseService("/waterplus/get_charger_name", getChargerByName);

    ros::Rate r(10);

    while(ros::ok())
    {
        PublishWaypointsMarker();
        ros::spinOnce();
        r.sleep();
    }

    return 0;
}

<|start_filename|>robot/src/drivers/waterplus_map_tools/include/UDPClient.h<|end_filename|>
#ifndef _EXS_UDPCLIENT_H
#define _EXS_UDPCLIENT_H
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
void InitUDPClient(char* inServerIP, int inPort);
void UDPClientSend(unsigned char* inData,int inLen);
void SendRobotState(int inID, int inState, float inX, float inY, float inAngle);
#endif


<|start_filename|>robot/src/drivers/waterplus_map_tools/src/network/UDPClient.c<|end_filename|>
/*!
 @header     UDPClient.c
 @abstract   UDP客户端
 @author     张万杰
 */
#include "UDPClient.h"

static char arRemoteIP[16] = "127.0.0.1";
static int nRemotePort;
static int s,len,addr_len;
static struct sockaddr_in addr;
static unsigned char arSendBuf[1024];
static int bInited = 0;

void InitUDPClient(char* inServerIP, int inPort)
{
	printf("[UDP_Client]InitUDPClient(IP = %s port = %d )...\n",inServerIP,inPort);
	memcpy(arRemoteIP,inServerIP,16);
	nRemotePort = inPort;

    addr_len =sizeof(struct sockaddr_in);
    /* 建立socket*/
    if((s = socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP))<0){
        perror("socket");
        return;
    }
    /* 填写sockaddr_in*/
    bzero(&addr,sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_port = htons(nRemotePort);
    addr.sin_addr.s_addr = inet_addr(arRemoteIP);

	bInited = 1;
}

void WordToBytes(int inVal,unsigned char* inDest)
{
    inDest[0] = (unsigned char )(inVal >> 8);
    inDest[1] = (unsigned char )(inVal);
}

void Int32ToBytes(int inVal,unsigned char* inDest)
{
    inDest[0] = (unsigned char )(inVal >> 24);
    inDest[1] = (unsigned char )(inVal >> 16);
    inDest[2] = (unsigned char )(inVal >> 8);
    inDest[3] = (unsigned char )(inVal);
}

void FloatToBytes(float inVal,unsigned char* inDest)
{
    memcpy(inDest,(unsigned char*)&inVal,sizeof(float));
}


void ReportResult(int inX,int inY,int inSum)
{
    arSendBuf[0] = 0x55;
    arSendBuf[1] = 0xaa;
    arSendBuf[2] = 13;      //len
    arSendBuf[3] = 0x00;    //flag
    //x
    WordToBytes(inX,&arSendBuf[4]);
    //y
    WordToBytes(inY,&arSendBuf[6]);
    //sum
    Int32ToBytes(inSum,&arSendBuf[8]);

    arSendBuf[arSendBuf[2]-1] = 0;
    int i=0;
    for(i=0;i<arSendBuf[2]-1;i++)
    {
        arSendBuf[arSendBuf[2]-1] += arSendBuf[i];
    }

    UDPClientSend(arSendBuf,arSendBuf[2]);
}

void UDPClientSend(unsigned char* inData,int inLen)
{
	if(bInited == 0)
	return;
    //printf("UDPClientSend( len = %d )...\n",inLen);
    /*printf("[UDP_Send] ");
    int i =0;
    for(i=0;i<inLen;i++)
    {
        printf("%.2X ",inData[i]);
    }
    printf("\n");*/
    /* 将字符串传送给server端*/
    sendto(s,inData,inLen,0,(struct sockaddr *)&addr,addr_len);
}

void SendRobotState(int inID, int inState, float inX, float inY, float inAngle)
{
    arSendBuf[0] = 0x55;
    arSendBuf[1] = 0xaa;
    arSendBuf[2] = inID;      
    arSendBuf[3] = inState;
    
    FloatToBytes(inX,&arSendBuf[4]);
    FloatToBytes(inY,&arSendBuf[8]);
    FloatToBytes(inAngle,&arSendBuf[12]);

    UDPClientSend(arSendBuf,16);
}


<|start_filename|>robot/src/drivers/xfyun_waterplus/src/wp_voice_cmd_cn.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* @author <NAME>                                             */
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>

#define CMD_STOP        0
#define CMD_FORWARD     1
#define CMD_BACKWARD    2  
#define CMD_LEFT        3
#define CMD_RIGHT       4   

#define CMD_DURATION    30

static ros::Publisher vel_pub;
static ros::Publisher spk_pub;
static int nCmd = CMD_STOP;
static int nCount = 0;

void KeywordCB(const std_msgs::String::ConstPtr & msg)
{
    //ROS_WARN("[KeywordCB] - %s",msg->data.c_str());
    geometry_msgs::Twist vel_cmd;
    vel_cmd.linear.x = 0;
    vel_cmd.linear.y = 0;
    vel_cmd.linear.z = 0;
    vel_cmd.angular.x = 0;
    vel_cmd.angular.y = 0;
    vel_cmd.angular.z = 0;

    bool bCmd = false;
    int nFindIndex = 0;
    nFindIndex = msg->data.find("前");
    if( nFindIndex >= 0 )
    {
        //ROS_WARN("[KeywordCB] - move x");
        bCmd = true;
        nCmd = CMD_FORWARD;
    }
    nFindIndex = msg->data.find("后");
    if( nFindIndex >= 0 )
    {
        //ROS_WARN("[KeywordCB] - move -x");
        bCmd = true;
        nCmd = CMD_BACKWARD;
    }
    nFindIndex = msg->data.find("左");
    if( nFindIndex >= 0 )
    {
        //ROS_WARN("[KeywordCB] - move y");
        bCmd = true;
        nCmd = CMD_LEFT;
    }

    nFindIndex = msg->data.find("右");
    if( nFindIndex >= 0 )
    {
        //ROS_WARN("[KeywordCB] - move -y");
        bCmd = true;
        nCmd = CMD_RIGHT;
    }

    nFindIndex = msg->data.find("停");
    if( nFindIndex >= 0 )
    {
        //ROS_WARN("[KeywordCB] - stop");
        bCmd = true;
        nCmd = CMD_STOP;
    }


    std_msgs::String strSpeak;
    if(bCmd == true)
    {
        nCount = CMD_DURATION;
        strSpeak.data = "好的";
        spk_pub.publish(strSpeak);
    }
    else
    {
        //strSpeak = *msg;
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "wp_voice_cmd_cn");

    ros::NodeHandle n;
    ros::Subscriber sub_sr = n.subscribe("/xfyun/iat", 10, KeywordCB);
    spk_pub = n.advertise<std_msgs::String>("/xfyun/tts", 20);
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    ros::Rate r(10);
    while(ros::ok())
    {
        geometry_msgs::Twist vel_cmd;
        if(nCount > 0)
        {
            nCount --;
            if(nCmd == CMD_FORWARD)
            {
                vel_cmd.linear.x = 0.1;
                //ROS_WARN("forward");
            }
            if(nCmd == CMD_BACKWARD)
            {
                vel_cmd.linear.x = -0.1;
                //ROS_WARN("backward");
            }
            if(nCmd == CMD_LEFT)
            {
                vel_cmd.angular.z = 0.1;
                //ROS_WARN("left");
            }
            if(nCmd == CMD_RIGHT)
            {
                vel_cmd.angular.z = -0.1;
                //ROS_WARN("right");
            }
            if(nCmd == CMD_STOP)
            {
                vel_cmd.linear.x = 0;
                vel_cmd.linear.y = 0;
                vel_cmd.angular.z = 0;
                //ROS_WARN("stop");
            }
        }
        else
        {
            nCmd = CMD_STOP;
            vel_cmd.linear.x = 0;
            vel_cmd.linear.y = 0;
            vel_cmd.angular.z = 0;
            //ROS_WARN("stop");
        }
        vel_pub.publish(vel_cmd);
        ros::spinOnce();
        r.sleep();
    }

    return 0;
}

<|start_filename|>robot/src/fight_with_hair_ui/src/usercontrol.cpp<|end_filename|>
#include "../include/fight_with_hair_ui/usercontrol.h"
#include "ui_usercontrol.h"
#include "../include/fight_with_hair_ui/mainframe.h"
#include <QMessageBox>
#include <QKeyEvent>
#include <QString>
#include <QCloseEvent>

userControl::userControl(QWidget *parent) :
  QWidget(parent),
  ui(new Ui::userControl)
{
  ui->setupUi(this);
  this->setFixedSize(this->geometry().width(),this->geometry().height());
  QString str = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch user_listen keyboard_user_listen.launch'&";
  system(str.toLatin1().data());
//  render_panel_ = new rviz::RenderPanel;
//  ui->mapdisplay_layout->addWidget(render_panel_);
//  manager_ = new rviz::VisualizationManager(render_panel_);

//  render_panel_->initialize(manager_->getSceneManager(), manager_);
//  manager_->initialize();
//  //manager_->load(config);
//  manager_->startUpdate();
//  manager_->setFixedFrame("/base_footprint");


  x_speed = new CCtrlDashBoard(ui->speed_x_widget);
  x_speed->setGeometry(ui->speed_x_widget->rect());
  x_speed->setValue(0);
  x_speed->setUnit(0);
  y_speed = new CCtrlDashBoard(ui->speed_y_widget);
  y_speed->setGeometry(ui->speed_y_widget->rect());
  y_speed->setValue(0);
  y_speed->setUnit(0);
  z_speed = new CCtrlDashBoard(ui->speed_z_widget);
  z_speed->setGeometry(ui->speed_z_widget->rect());
  z_speed->setValue(0);
  z_speed->setUnit(0);
}

userControl::~userControl()
{
  delete ui;
}

void userControl::on_logout_clicked()
{
  QMessageBox::StandardButton choice=QMessageBox::question(this,"确认","确定要退出吗？",QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes);
  if(choice==QMessageBox::Yes){
    //    QString str = "gnome-terminal -x bash -c 'source ~/.bashrc; rosnode kill all'&";
    //    system(str.toLatin1().data());
    this->close();
    QString string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /arm_move;"
                     "rosnode kill /basic_avoidance;"
                     "rosnode kill /basic_move;"
                     "rosnode kill /my_joy;"
                     "rosnode kill /keyboard_listen;"
                     "rosnode kill /user_listen;"
                     "'&";
    //rosnode kill /robot_state_publisher;
//    system(string.toLatin1().data());
    string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /joint_state_publisher;rosnode kill /robot_state_publisher;'&";
//    system(string.toLatin1().data());
    QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
    system(killString.toLatin1().data());
    killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
    system(killString.toLatin1().data());
    QApplication::quit();
  }
}

void userControl::on_returnMain_clicked()
{
  //  mainframe *main=new mainframe;
  //  main->move(550,200);
  //  main->show();
  QString string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /arm_move;"
                   "rosnode kill /basic_avoidance;"
                   "rosnode kill /basic_move;"
                   "rosnode kill /my_joy;"
                   "rosnode kill /keyboard_listen;"
                   "rosnode kill /user_listen;"
                   "rosnode kill /slam_lidar_filter;"
                   "rosnode kill /wpb_home_core;"
                   "'&";
  //rosnode kill /robot_state_publisher;
//  system(string.toLatin1().data());
  QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
  system(killString.toLatin1().data());
  killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
  system(killString.toLatin1().data());
  this->close();
}

void userControl::keyPressEvent(QKeyEvent *event){
  //键盘按下事件
  if(event->key() == Qt::Key_Escape){
    on_logout_clicked();
  }else if(event->key()==Qt::Key_W){
    ui->forward->setStyleSheet("color:red;");
    if(clickForward){
      if (x_send < 1){
        x_send+=0.01;
      }
      emit movemsg(x_send,0,0);
      showSpeed(x_send,0,0);
      clickForward = true;
      clickBack = false;
    }else{
      sendDataInit();
      emit movemsg(x_send,0,0);
      showSpeed(x_send,0,0);
      clickForward = true;
      clickBack = false;
    }
  }else if(event->key()==Qt::Key_A){
    ui->left->setStyleSheet("color:red;");
    if(clickTurnLeft){
      if (z_send < 1){
        z_send+=0.01;
      }
      emit movemsg(0,0,z_send );
      showSpeed(0,0, z_send);
      clickTurnLeft = true ;
    }else{
      sendDataInit();
      emit movemsg(0,0,z_send);
      showSpeed(0,0, z_send);
      clickTurnLeft = true;
    }
  }else if(event->key()==Qt::Key_X){
    ui->backward->setStyleSheet("color:red;");
    if(clickBack){
      if (x_send < 1){
        x_send+=0.01;
      }
      emit movemsg(x_send * (-1),0,0);
      showSpeed(x_send * (-1),0,0);
      clickForward = false;
      clickBack = true;
    }else{
      sendDataInit();
      emit movemsg(x_send * (-1),0,0);
      showSpeed(x_send * (-1),0,0);
      clickForward = false;
      clickBack = true;
    }
  }else if(event->key()==Qt::Key_D){
    ui->right->setStyleSheet("color:red;");
    if(clickTurnRight){
      if (z_send < 1){
        z_send+=0.01;
      }
      emit movemsg(0,0,z_send * (-1));
      showSpeed(0,0, z_send * (-1));
      clickTurnRight = true ;
    }else{
      sendDataInit();
      emit movemsg(0,0,z_send * (-1));
      showSpeed(0,0, z_send * (-1));
      clickTurnRight = true;
    }
  }else if(event->key()==Qt::Key_S){
    ui->stop->setStyleSheet("color:red;");
    emit movemsg(0,0,0);
    showSpeed(0,0,0);
    sendDataInit();
  }else if(event->key()==Qt::Key_I){
    ui->up->setStyleSheet("color:red;");
    emit armmsg(1,0.01,0,0);
    sendDataInit();
  }else if(event->key()==Qt::Key_J){
    ui->grab->setStyleSheet("color:red;");
    emit armmsg(0,0,1,1);
    sendDataInit();
  }else if(event->key()==Qt::Key_K){
    ui->down->setStyleSheet("color:red;");
    emit armmsg(2,0.01,0,0);
    sendDataInit();
  }else if(event->key()==Qt::Key_L){
    ui->release->setStyleSheet("color:red;");
    emit armmsg(0,0,2,1);
    sendDataInit();
  }else if(event->key()==Qt::Key_Q){
    ui->left_forward->setStyleSheet("color:red;");
    emit movemsg(0.3,0,0.3);
    showSpeed(0.3,0,0.3);
    sendDataInit();
  }else if(event->key()==Qt::Key_E){
    ui->right_forward->setStyleSheet("color:red;");
    emit movemsg(0.3,0,-0.3);
    showSpeed(0.3,0,-0.3);
    sendDataInit();
  }else if(event->key()==Qt::Key_Z){
    ui->left_back->setStyleSheet("color:red;");
    emit movemsg(-0.3,0,0.3);
    showSpeed(-0.3,0,0.3);
    sendDataInit();
  }else if(event->key()==Qt::Key_C){
    ui->right_back->setStyleSheet("color:red;");
    emit movemsg(-0.3,0,-0.3);
    showSpeed(-0.3,0,-0.3);
    sendDataInit();
  }else if(event->key()==Qt::Key_O){
    ui->recover->setStyleSheet("color:red;");
    emit armmsg(0,0,0,0);
    sendDataInit();
  }
}
void userControl::keyReleaseEvent(QKeyEvent *event) {
  //键盘松开事件
  if(event->key()==Qt::Key_W){
    ui->forward->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_A){
    ui->left->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_X){
    ui->backward->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_D){
    ui->right->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_S){
    ui->stop->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_I){
    ui->up->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_J){
    ui->grab->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_K){
    ui->down->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_L){
    ui->release->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_Q){
    ui->left_forward->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_E){
    ui->right_forward->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_Z){
    ui->left_back->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_C){
    ui->right_back->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_O){
    ui->recover->setStyleSheet("color:black;");
  }
}

void userControl::on_forward_clicked()
{
  if(clickForward){
    if (x_send < 1){
      x_send+=0.01;
    }
    emit movemsg(x_send,0,0);
    showSpeed(x_send,0,0);
    clickForward = true;
    clickBack = false;
  }else{
    sendDataInit();
    emit movemsg(x_send,0,0);
    showSpeed(x_send,0,0);
    clickForward = true;
    clickBack = false;
  }
}

void userControl::on_left_clicked()
{
  emit movemsg(0,0,0.6);
  showSpeed(0,0,0.6);
  sendDataInit();
}

void userControl::on_backward_clicked()
{
  if(clickBack){
    if (x_send < 1){
      x_send+=0.01;
    }
    emit movemsg(x_send * (-1),0,0);
    showSpeed(x_send * (-1),0,0);
    clickForward = false;
    clickBack = true;
  }else{
    sendDataInit();
    emit movemsg(x_send * (-1),0,0);
    showSpeed(x_send * (-1),0,0);
    clickForward = false;
    clickBack = true;
  }
}

void userControl::on_right_clicked()
{
  if(clickTurnRight){
    if (z_send < 1){
      z_send+=0.01;
    }
    emit movemsg(0,0,z_send * (-1));
    showSpeed(0,0, z_send * (-1));
    clickTurnRight = true ;
  }else{
    sendDataInit();
    emit movemsg(0,0,z_send * (-1));
    showSpeed(0,0, z_send * (-1));
    clickTurnRight = true;
  }
}

void userControl::on_stop_clicked()
{
  emit movemsg(0,0,0);
  showSpeed(0,0,0);
  sendDataInit();
}

void userControl::on_left_forward_clicked()
{
  emit movemsg(0.3,0,0.3);
  showSpeed(0.3,0,0.3);
  sendDataInit();
}

void userControl::on_right_forward_clicked()
{
  emit movemsg(0.3,0,-0.3);
  showSpeed(0.3,0,-0.3);
  sendDataInit();
}

void userControl::on_left_back_clicked()
{
  emit movemsg(-0.3,0,0.3);
  showSpeed(-0.3,0,0.3);
  sendDataInit();
}

void userControl::on_right_back_clicked()
{
  emit movemsg(-0.3,0,-0.3);
  showSpeed(-0.3,0,-0.3);
  sendDataInit();
}

void userControl::on_up_clicked()
{
  emit armmsg(1,0.01,0,0);
  sendDataInit();
}

void userControl::on_down_clicked()
{
  emit armmsg(2,0.01,0,0);
  sendDataInit();
}

void userControl::on_grab_clicked()
{
  emit armmsg(0,0,1,1);
  sendDataInit();
}

void userControl::on_release_clicked()
{
  emit armmsg(0,0,2,1);
  sendDataInit();
}

void userControl::on_open_rviz_clicked()
{


}

void userControl::closeEvent(QCloseEvent *event)
{
  main->show();
  emit userControl_finish();
}

void userControl::setMain(fight_with_hair_ui::MainWindow *m){
  main=m;
}

void userControl::showSpeed(float x, float y, float z){
  x_speed->setValue(x * 50);
  y_speed->setValue(y * 50);
  z_speed->setValue(z * (-50));
}

void userControl::sendDataInit(){
  clickForward = false;
  clickBack = false;
  clickTurnLeft = false;
  clickTurnRight = false;
  x_send = 0.2;
  y_send = 0;
  z_send = 0.5;

}

void userControl::on_open_rviz_image_clicked()
{

}

void userControl::on_recover_clicked()
{
    sendDataInit();
    emit armmsg(0,0,0,0);
}


<|start_filename|>robot/src/drivers/waterplus_map_tools/src/wp_nav_odom_report.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* @author <NAME>                                             */

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>
#include <string>
extern "C" {
#include "UDPClient.h"
}

static int Robot_ID = 1;    //机器人ID

int main(int argc, char** argv)
{
    ros::init(argc, argv, "wp_nav_odom_report");
    InitUDPClient((char*)"192.168.1.110", 20180); //服务器IP地址
    
    ros::NodeHandle nh;
    ros::Rate r(10.0);

    tf::TransformListener listener;
    tf::StampedTransform transform;

    while(ros::ok())
    {
        try
        {
            listener.waitForTransform("/map","/base_footprint",  ros::Time(0), ros::Duration(10.0) );
            listener.lookupTransform("/map","/base_footprint", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("[lookupTransform] %s",ex.what());
            break;
        }
        float tx = transform.getOrigin().x();
        float ty = transform.getOrigin().y();
        tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(transform.getRotation() , tf::Point(tx, ty, 0.0)), ros::Time::now(), "map");
        geometry_msgs::PoseStamped robot_pos;
        tf::poseStampedTFToMsg(p, robot_pos);

        tf::Quaternion quat(robot_pos.pose.orientation.x,robot_pos.pose.orientation.y,robot_pos.pose.orientation.z,robot_pos.pose.orientation.w);
        double roll,pitch,yaw;
        tf::Matrix3x3(quat).getRPY(roll,pitch,yaw);
        ROS_WARN("[Robot Pos]( %.2f , %.2f ) - %.2f" , robot_pos.pose.position.x, robot_pos.pose.position.y, yaw);

        SendRobotState(Robot_ID, 1, robot_pos.pose.position.x, robot_pos.pose.position.y, yaw);

        ros::spinOnce();
        r.sleep();
    }

    return 0;
}

<|start_filename|>robot/src/drivers/wpb_home/wpb_home_tutorials/src/wpb_home_imu_turn.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*!******************************************************************
 @author     ZhangWanjie
 ********************************************************************/

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Twist.h>

static bool bFirstFrame = true;
static float fOriginYaw = 0.0f;
static ros::Publisher vel_pub;
static geometry_msgs::Twist vel_cmd;

void imuCallback(const sensor_msgs::Imu::ConstPtr& msg)
{
    tf::Quaternion q(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w);
    float fCurYaw = tf::getYaw(q)*180/3.1415;

    if(bFirstFrame == true)
    {
        fOriginYaw = fCurYaw;
        bFirstFrame = false;
    }
    else
    {
        float dYaw = fCurYaw - fOriginYaw;
        while (dYaw > 360)
        {
            dYaw -= 360;
        }
        while (dYaw < 0)
        {
            dYaw += 360;
        }

        if(fabs(dYaw - 180) < 5)
        {
            vel_cmd.angular.z = 0;
            vel_pub.publish(vel_cmd);
        }
        else
        {
            if(dYaw > 180)
            {
                vel_cmd.angular.z = -0.3;
                vel_pub.publish(vel_cmd);
            }
            else
            {
                vel_cmd.angular.z = 0.3;
                vel_pub.publish(vel_cmd);
            }
        }

        printf("[wpb_home_imu_turn] CurYaw= %.2f  dYaw = %.2f\n",fCurYaw, dYaw);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc,argv, "wpb_home_imu_turn"); 
    ROS_INFO("[wpb_home_imu_turn]");

    ros::NodeHandle n;
    ros::Subscriber imu_sub = n.subscribe("imu/data_raw", 100, imuCallback);
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    vel_cmd.linear.x = 0;
    vel_cmd.linear.y = 0;
    vel_cmd.linear.z = 0;
    vel_cmd.angular.x = 0;
    vel_cmd.angular.y = 0;
    vel_cmd.angular.z = 0;
    vel_pub.publish(vel_cmd);
    ros::spin();

    return 0;
}

<|start_filename|>robot/src/user_listen/src/user_listen.cpp<|end_filename|>
#include "ros/ros.h"
#include "sensor_msgs/Joy.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "basic/moveMsg.h"
#include "arm/armMsg.h"
#include <iostream>

using namespace std;

ros::Publisher vel_pub;
basic::moveMsg vel;
ros::Publisher arm_pub;
arm::armMsg arm_vel;


void setmoveMsg(float x, float y, float z){
    vel.x = x;
    vel.y = y;
    vel.z = z;
    vel_pub.publish(vel);
}

void setArmMsg(float dire_v, float len, float dire_h, float degree){
    arm_vel.direction_v = dire_v;
    arm_vel.length = len;
    arm_vel.direction_h = dire_h;
    arm_vel.degree = degree;
    arm_pub.publish(arm_vel);
}

void keyboardListen(const std_msgs::String::ConstPtr &msg) {
    //std::cout << "------------" << std::endl;
    //ROS_INFO("Keyboard %s", msg->data.c_str());
    char key = msg->data[0];
    if (key == 'a' || key == 'A') {
        setmoveMsg(0,0,0.3);
    }
    else if (key == 'd' || key == 'D'){
        setmoveMsg(0,0,-0.3);
    }
    else if (key == 'w' || key == 'W'){
        setmoveMsg(0.3,0,0);
    }
    else if (key == 'x' || key == 'X'){
        setmoveMsg(-0.3,0,0);
    }
    else if (key == 's' || key == 'S'){
        setmoveMsg(0,0,0);
    }
    else if (key == 'q' || key == 'Q'){
        setmoveMsg(0.3,0,0.3);
    }
    else if (key == 'e' || key == 'E'){
        setmoveMsg(-0.3,0,0);
    }
    else if (key == 'z' || key == 'Z'){
        setmoveMsg(-0.3,0,0.3);
    }
    else if (key == 'c' || key == 'C'){
        setmoveMsg(-0.3,0,-0.3);
    }
    else if (key == 'i' || key == 'I'){ //up
        setArmMsg(1,0.01,0,0);
    }
    else if (key == 'k' || key == 'K'){
        setArmMsg(2,0.01,0,0);
    }
    else if (key == 'l' || key == 'L'){
        setArmMsg(0,0,2,0.1);
    }
    else if (key == 'j' || key == 'J'){
        setArmMsg(0,0,1,0.1);
    }
    else if (key == 'o' || key == 'O'){
        setArmMsg(0,0,0,0);
    }
}

void joyListen(const sensor_msgs::Joy::ConstPtr& msg){
    if (msg->buttons[0]) {
        setArmMsg(1,0.01,0,0);
    }
    else if (msg->buttons[1]) {
        setArmMsg(2,0.01,0,0);
    }
    else if (msg->buttons[2]) {
        setArmMsg(0,0,1,0.1);
    }
    else if (msg->buttons[3]) {
        setArmMsg(0,0,2,0.1);
    }
    else {
        setmoveMsg(msg->axes[1],msg->axes[0],msg->axes[3]);
    }
}


int main(int argc, char **argv){
    ros::init(argc, argv, "user_listen");
    ros::NodeHandle nh;
    vel_pub = nh.advertise<basic::moveMsg>("/fight_with_hair/basic_move/vel", 10);
    arm_pub = nh.advertise<arm::armMsg>("/fight_with_hair/arm_move",10);

    ros::Subscriber keyboard_sub = nh.subscribe("/fight_with_hair/user_listen/keyboard", 10, keyboardListen);
    ros::Subscriber joy_sub = nh.subscribe("joy", 10, joyListen);
    ros::spin();
    return 0;
}


<|start_filename|>robot/src/fight_with_hair_ui/src/follow.cpp<|end_filename|>
#include "../include/fight_with_hair_ui/follow.h"
#include "ui_follow.h"
#include <QMessageBox>
#include <QPixmap>
#include <QCloseEvent>

Follow::Follow(bool isSim,QWidget *parent) :
  QWidget(parent),
  ui(new Ui::Follow)
{
  ui->setupUi(this);
  this->setFixedSize(this->geometry().width(),this->geometry().height());
  QString followStr;
  if (isSim){
    followStr = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch fwh_follow fwh_follow_t.launch'&";
  }else{
    followStr = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch fwh_follow fwh_follow_l.launch'&";
  }
  system(followStr.toLatin1().data());

  QPixmap *ro=new QPixmap(":/images/follow_robot.jpg");
  ro->scaled(ui->pic->size(),Qt::KeepAspectRatio);
  ui->pic->setScaledContents(true);
  ui->pic->setPixmap(*ro);
}

Follow::~Follow()
{
  delete ui;
}

void Follow::on_returnMain_clicked()
{
  QString string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /fwh_follow_yz'&";
//  system(string.toLatin1().data());
  QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
  system(killString.toLatin1().data());
  killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
  system(killString.toLatin1().data());
  this->close();
}

void Follow::on_logout_clicked()
{
  QMessageBox::StandardButton choice=QMessageBox::question(this,"确认","确定要退出吗？",QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes);
  if(choice==QMessageBox::Yes){
    this->close();
    QString string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /fwh_follow_yz'&";

//    system(string.toLatin1().data());
    string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /joint_state_publisher;rosnode kill /robot_state_publisher;'&";
//    system(string.toLatin1().data());
    QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
    system(killString.toLatin1().data());
    killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
    system(killString.toLatin1().data());
    QApplication::quit();
  }
}

void Follow::on_follow_begin_clicked()
{
  emit send_follow_begin_msg();
}

void Follow::on_follow_end_clicked()
{
  emit send_follow_end_msg();
}

void Follow::closeEvent(QCloseEvent *event){
  main->show();
  emit follow_finish();
}

void Follow::setMain(fight_with_hair_ui::MainWindow *m){
  main=m;
}


<|start_filename|>robot/src/basic/src/basic_avoidance.cpp<|end_filename|>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/LaserScan.h>
#include "basic/checksrv.h"
#include <algorithm>
#include <math.h>
float ranges[400];
float MAX_DIS = 1.0;
float MIN_DIS = 0.2;

void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    int nNum = scan->ranges.size();
    for(int i=0 ; i<nNum ; i++)
    {
        ranges[i] = scan->ranges[i]; 
    }
}

int calAngle(float x, float y){
    float x_abs = fabs(x);
    float y_abs = fabs(y);
    float angle_pi = M_PI / 2;
    if (x_abs != 0){
        angle_pi = atan2(y_abs,x_abs);
    }

    int angle_d = int(angle_pi / M_PI * 180);
    ROS_INFO("The angle_pi is %.2f, %d", angle_pi, angle_d);
    if (x >= 0 && y >= 0){
        angle_d = 180 - angle_d;
    }else if (x >= 0 and y < 0) {
        angle_d = 180 + angle_d;
    }else if (x < 0 and y >= 0){
        angle_d = angle_d;
    }else if (x < 0 and y < 0){
        angle_d = 360-angle_d;
    }

    return angle_d;
}

bool check(basic::checksrv::Request &request, basic::checksrv::Response &response){
    int angle = calAngle(request.x,request.y);
    ROS_INFO("The x is : %.2f", request.x);
    ROS_INFO("The angle is : %d, the dis is %.2f", angle, ranges[angle]);
    if (ranges[angle] >= MAX_DIS){
        ROS_INFO("The response should be: %.2f,%.2f",request.x,request.y);
        response.x_new = request.x;
        response.y_new = request.y;
    }
    else if (ranges[angle] < MAX_DIS && ranges[angle] > MIN_DIS){\
        ROS_INFO("There's obstacle %.2f front!!", ranges[angle]);
        float k = (ranges[angle] - MIN_DIS) / (MAX_DIS - MIN_DIS);
        response.x_new = request.x * k;
        response.y_new = request.y * k;
    }
    else {
        ROS_INFO("Too close!!%.2f", ranges[angle]);
        response.x_new = 0;
        response.y_new = 0;
    }
    return true;
}

int main(int argc, char** argv)
{
    ros::init(argc,argv,"basic_avoidance");
    ROS_INFO("lidar_data_node start!");
    ros::NodeHandle nh;
    ros::ServiceServer server = nh.advertiseService("/fight_with_hair/basic_move/avoid", check);
    ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &lidarCallback);
    ros::spin();
    return 0;
}


<|start_filename|>robot/src/drivers/wpb_home_apps/src/scripts/fwh_script.cpp<|end_filename|>
#include "f_fwh_script.h"
FwhScript::FwhScript()
{
    
}

FwhScript::~FwhScript()
{

}

void FwhScript::Queue()
{
    stAct newAct;

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "开始抓取饮料";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_GRAB;
    newAct.strTarget = "";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "饮料抓取完毕";
    newAct.nDuration = 2;
    arAct.push_back(newAct);

    newAct.nAct = ACT_GOTO;
    newAct.strTarget = "master";
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "饮料已取回,主人请慢用";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_PASS;
    newAct.strTarget = "";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "任务完成";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

}

<|start_filename|>robot/src/drivers/wpb_home/wpb_home_tutorials/src/wpb_home_pointcloud_cluster.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*!******************************************************************
 @author     ZhangWanjie
 ********************************************************************/
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_listener.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/search/kdtree.h>

static tf::TransformListener *tf_listener; 

void PointcloudCB(const sensor_msgs::PointCloud2 &input)
{
    // 将点云数值从相机坐标系转换到机器人坐标系
    bool result = tf_listener->waitForTransform("/base_footprint", input.header.frame_id, input.header.stamp, ros::Duration(1.0)); 
    if(result == false)
    {
        return;
    }
    sensor_msgs::PointCloud2 pc_footprint;
    pcl_ros::transformPointCloud("/base_footprint", input, pc_footprint, *tf_listener);

    // 将点云数据从ROS格式转换到PCL格式
    pcl::PointCloud<pcl::PointXYZRGB> cloud_src;
    pcl::fromROSMsg(pc_footprint , cloud_src);
   
    // 对设定范围内的点云进行截取
    pcl::PassThrough<pcl::PointXYZRGB> pass;
    // 截取x轴方向，前方0.5米到1.5米内的点云
    pass.setInputCloud (cloud_src.makeShared());
    pass.setFilterFieldName ("x");
    pass.setFilterLimits (0.5, 1.5);
    pass.filter (cloud_src);
    // 截取y轴方向，左侧0.5米到右侧0.5米内的点云
    pass.setInputCloud (cloud_src.makeShared());
    pass.setFilterFieldName ("y");
    pass.setFilterLimits (-0.5, 0.5);
    pass.filter (cloud_src);
    // 截取z轴方向，高度0.5米到1.5米内的点云
    pass.setInputCloud (cloud_src.makeShared());
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0.5, 1.5);
    pass.filter (cloud_src);

    // 定义模型分类器
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::SACSegmentation<pcl::PointXYZRGB> segmentation;
    segmentation.setInputCloud(cloud_src.makeShared());
    segmentation.setModelType(pcl::SACMODEL_PLANE);
    segmentation.setMethodType(pcl::SAC_RANSAC);
    segmentation.setDistanceThreshold(0.05);
    segmentation.setOptimizeCoefficients(true);

    // 使用模型分类器进行检测
    pcl::PointIndices::Ptr planeIndices(new pcl::PointIndices);
    segmentation.segment(*planeIndices, *coefficients);

    // 统计平面点集的平均高度
    int point_num =  planeIndices->indices.size();
    float points_z_sum = 0;
    for(int i=0;i<point_num;i++)
    {
        int point_index = planeIndices->indices[i];
        points_z_sum += cloud_src.points[point_index].z;
    }
    float plane_height = points_z_sum/point_num;
    ROS_INFO("plane_height = %.2f",plane_height);

    // 对点云再次进行截取，只保留平面以上的部分
    pass.setInputCloud (cloud_src.makeShared());
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (plane_height + 0.2, 1.5);
    pass.filter (cloud_src);

    // 对截取后的点云进行欧式距离分割
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
    tree->setInputCloud (cloud_src.makeShared());		               // 输入截取后的点云
    std::vector<pcl::PointIndices> cluster_indices;	                        // 点云团索引
    pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;       // 欧式聚类对象
    ec.setClusterTolerance (0.02);			                                                 // 设置近邻搜索的搜索半径为2cm
    ec.setMinClusterSize (100);			                                                        // 设置一个聚类需要的最少的点数目为100
    ec.setMaxClusterSize (20000);			                                                // 设置一个聚类需要的最大点数目为20000
    ec.setSearchMethod (tree);			                                                      // 设置点云的搜索机制
    ec.setInputCloud (cloud_src.makeShared());                            // 输入截取后的点云
    ec.extract (cluster_indices);                                                               // 执行欧式聚类分割

    // 计算每个分割出来的点云团的中心坐标
    int object_num = cluster_indices.size();                                       // 分割出的点云团个数
    ROS_INFO("object_num = %d",object_num);
    for(int i = 0 ; i < object_num ; i ++)
    {
        int point_num =  cluster_indices[i].indices.size();                 // 点云团i中的点数
        float points_x_sum = 0;
        float points_y_sum = 0;
        float points_z_sum = 0;
        for(int j = 0 ; j < point_num ; j ++)
        {
            int point_index = cluster_indices[i].indices[j];
            points_x_sum += cloud_src.points[point_index].x;
            points_y_sum += cloud_src.points[point_index].y;
            points_z_sum += cloud_src.points[point_index].z;
        }
        float object_x = points_x_sum/point_num;
        float object_y = points_y_sum/point_num;
        float object_z = points_z_sum/point_num;
        ROS_INFO("object %d pos = ( %.2f , %.2f , %.2f)" ,i, object_x,object_y,object_z);
    }
 } 

int main(int argc, char **argv)
{
    ros::init(argc, argv, "wpb_home_pointcloud_cluster");
    tf_listener = new tf::TransformListener(); 
    ros::NodeHandle nh;
    ros::Subscriber pc_sub = nh.subscribe("/kinect2/sd/points", 10 , PointcloudCB);

    ros::spin();

    delete tf_listener; 

    return 0;

}

<|start_filename|>robot/src/fwh_obj/src/fwh_obj_detect.cpp<|end_filename|>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt32.h>
#include <std_msgs/Bool.h>
#include <wpb_home_behaviors/Coord.h>
#include <fwh_obj/tarobj.h>

static bool ifbegin = true;
static ros::Publisher obj_uploader;
static ros::Publisher tar_uploader;
float x[100],y[100],z[100];

void obj_start(const std_msgs::Bool::ConstPtr &msg){
    ifbegin=true;
    ROS_WARN("begin_got_from_ui!");
}

void obj_detec_upload(const wpb_home_behaviors::Coord::ConstPtr &msg){
    ROS_WARN("obj_got_from_ui! _stop");
    if (ifbegin == true) {
        ifbegin=false;
        ROS_WARN("obj_got_from_ui! _begin");
        int objNum = msg->name.size();
        std_msgs::UInt32 uptp;
        uptp.data=objNum;
        obj_uploader.publish(uptp);
        ROS_WARN("objNum = %d",objNum);
        for(int i=0 ; i < objNum ; i++){
            ROS_WARN("obj[%s]'s location is (%.2f , %.2f , %.2f)",msg->name[i].c_str(),msg->x[i],msg->y[i],msg->z[i]);
            x[i]=msg->x[i];
            y[i]=msg->y[i];
            z[i]=msg->z[i];
            ROS_WARN("temp[%d]'s location is (%.2f , %.2f , %.2f)",i,x[i],y[i],z[i]);
        }
    }
}

void obj_tar_upload(const std_msgs::UInt32::ConstPtr &msg){
    ROS_WARN("tar_num = %d",msg->data);
    fwh_obj::tarobj uptp;
    uptp.x=x[msg->data];
    uptp.y=y[msg->data];
    uptp.z=z[msg->data];
    ROS_WARN("tar's location is (%.2f , %.2f , %.2f)",uptp.x,uptp.y,uptp.z);
    tar_uploader.publish(uptp);
}

int main(int argc, char** argv){

    ros::init(argc, argv, "fwh_obj_detect");
    ROS_WARN("fwh_obj_detect start!");
    ros::NodeHandle nh;
    ros::Subscriber ifstart = nh.subscribe("fight_with_hair/uito/obj_begin", 1, obj_start);
    ros::Subscriber obj_sub = nh.subscribe("/wpb_home/objects_3d", 1, obj_detec_upload);
    obj_uploader = nh.advertise<std_msgs::UInt32>("fight_with_hair/toui/obj_num",1);
    ros::Subscriber tarnum_sub = nh.subscribe("fight_with_hair/uito/obj_tar", 1, obj_tar_upload);
    tar_uploader = nh.advertise<fwh_obj::tarobj>("/tograb/obj_target",1);

    
    ros::spin();
    return 0;

}


<|start_filename|>robot/src/drivers/wpb_home/wpb_home_behaviors/src/wpb_home_grab_server.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*!******************************************************************
 @author     ZhangWanjie
 ********************************************************************/

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/segmentation/extract_clusters.h>
#include <image_geometry/pinhole_camera_model.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include <sensor_msgs/Image.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/String.h>
#include <sound_play/SoundRequest.h>
#include <geometry_msgs/Pose2D.h>
#include <opencv2/highgui/highgui_c.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <iostream>
#include <vector>

// 抓取参数调节（单位：米）
static float grab_y_offset = 0.0f;          //抓取前，对准物品，机器人的横向位移偏移量
static float grab_lift_offset = 0.2f;       //手臂抬起高度的补偿偏移量
static float grab_forward_offset = 0.1f;    //手臂抬起后，机器人向前抓取物品移动的位移偏移量
static float grab_gripper_value = 0.032;    //抓取物品时，手爪闭合后的手指间距

#define STEP_WAIT           0
#define STEP_FIND_PLANE     1
#define STEP_PLANE_DIST     2
#define STEP_FIND_OBJ       3
#define STEP_OBJ_DIST       4
#define STEP_HAND_UP        5
#define STEP_FORWARD        6
#define STEP_GRAB           7
#define STEP_OBJ_UP         8
#define STEP_BACKWARD       9
#define STEP_DONE           10
static int nStep = STEP_WAIT;

static std::string pc_topic;
static ros::Publisher pc_pub;
static ros::Publisher marker_pub;
static ros::Publisher vel_pub;
static ros::Publisher mani_ctrl_pub;
static sensor_msgs::JointState mani_ctrl_msg;
static ros::Publisher result_pub;
static tf::TransformListener *tf_listener; 

void DrawBox(float inMinX, float inMaxX, float inMinY, float inMaxY, float inMinZ, float inMaxZ, float inR, float inG, float inB);
void DrawText(std::string inText, float inScale, float inX, float inY, float inZ, float inR, float inG, float inB);
void DrawPath(float inX, float inY, float inZ);
void RemoveBoxes();
void VelCmd(float inVx , float inVy, float inTz);

static visualization_msgs::Marker line_box;
static visualization_msgs::Marker line_follow;
static visualization_msgs::Marker text_marker;

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
ros::Publisher segmented_plane;
ros::Publisher segmented_objects;
ros::Publisher masking;
ros::Publisher color;
static std_msgs::String result_msg;

static ros::Publisher ctrl_pub;
static std_msgs::String ctrl_msg;
static geometry_msgs::Pose2D pose_diff;

static float fObjGrabX = 0;
static float fObjGrabY = 0;
static float fObjGrabZ = 0;
static float fMoveTargetX = 0;
static float fMoveTargetY = 0;
static bool nPlaneDistMode = 1;
cv::Mat rgb_image;

typedef struct stBoxMarker
{
    float xMax;
    float xMin;
    float yMax;
    float yMin;
    float zMax;
    float zMin;
}stBoxMarker;

static stBoxMarker boxMarker;
static stBoxMarker boxPlane;
static stBoxMarker obj_to_track;

static int nTimeDelayCounter = 0;
static int nFrameCount = 0;
static float fLastPlaneHeight = 0.5;
static float fPlaneHeight = 0;
static int nPlaneHeightCounter = 0;

static float fPlaneDist = 0;
static float fTargetPlaneDist = 0.6;    //与桌子之间的目标距离
static float fTargetGrabX = 0.8;        //抓取时目标物品的x坐标
static float fTargetGrabY = 0.0;        //抓取时目标物品的y坐标

static std::vector<stBoxMarker> vObj;        
static stBoxMarker boxLastObject;
static int nObjDetectCounter = 0;
static int nTimeOut = 0;

void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
{
    if(nStep == STEP_WAIT)
        return;

    //to footprint
    sensor_msgs::PointCloud2 pc_footprint;
    tf_listener->waitForTransform("/base_footprint", input.header.frame_id, input.header.stamp, ros::Duration(5.0));  //return value always  false!
    pcl_ros::transformPointCloud("/base_footprint", input, pc_footprint, *tf_listener);

    //source cloud
    pcl::PointCloud<pcl::PointXYZRGB> cloud_src;
    pcl::fromROSMsg(pc_footprint , cloud_src);
    //ROS_INFO("cloud_src size = %d",cloud_src.size()); 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source_ptr;
    //////////////////////////////////////////////////
    //1、原始点云
    //cloud_source_ptr = cloud_src.makeShared(); 
    /////////////////////////////////////////////////
    //2、新构建一个点云，只取一部分
    pcl::PointCloud<pcl::PointXYZRGB> new_point_cloud;
    int nSize = cloud_src.points.size();
    for(int i=0;i<nSize;i++)
    {
        pcl::PointXYZRGB p = cloud_src.points[i];
        if(
            p.x < 1.2 &&
            p.y > -1.0 &&
            p.y < 1.0  &&
            p.z > 0.6 &&
            p.z < 1.5 
        )
        {
            new_point_cloud.push_back(p);
        }
    }
    cloud_source_ptr = new_point_cloud.makeShared(); 
    //////////////////////////////////////////////////
    
    /*一、数据处理 *************************************************************************************************/
    if( nStep == STEP_FIND_PLANE || nStep == STEP_FIND_OBJ)
    {
        // Process
        //ROS_INFO("Cloud: width = %d, height = %d\n", input.width, input.height);
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr plane(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr convexHull(new pcl::PointCloud<pcl::PointXYZRGB>);

        // Get the plane model, if present.
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
        pcl::SACSegmentation<pcl::PointXYZRGB> segmentation;
        segmentation.setInputCloud(cloud_source_ptr);
        segmentation.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
        segmentation.setMethodType(pcl::SAC_RANSAC);
        segmentation.setDistanceThreshold(0.005);
        segmentation.setOptimizeCoefficients(true);
        Eigen::Vector3f axis = Eigen::Vector3f(0.0,0.0,1.0);
        segmentation.setAxis(axis);
        segmentation.setEpsAngle(  10.0f * (M_PI/180.0f) );
        pcl::PointIndices::Ptr planeIndices(new pcl::PointIndices);
        segmentation.segment(*planeIndices, *coefficients);
        //ROS_INFO_STREAM("Num_Of_Planes = " << planeIndices->indices.size());
        pcl::ExtractIndices<pcl::PointXYZRGB> extract;

        RemoveBoxes();
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
        int i = 0, nr_points = (int) cloud_source_ptr->points.size ();
        // While 30% of the original cloud is still there
        while (cloud_source_ptr->points.size () > 0.03 * nr_points)
        {
            // Segment the largest planar component from the remaining cloud
            segmentation.setInputCloud (cloud_source_ptr);
            segmentation.segment (*planeIndices, *coefficients);
            if (planeIndices->indices.size () == 0)
            {
                ROS_WARN("Could not estimate a planar model for the given dataset.");
                break;
            }

            // Extract the planeIndices
            extract.setInputCloud (cloud_source_ptr);
            extract.setIndices (planeIndices);
            extract.setNegative (false);
            extract.filter (*plane);
            float plane_height = 0;
            for(int k=0; k<plane->width * plane->height ;k ++ )
            {
                plane_height += plane->points[k].z;
            }
            plane_height /= plane->width * plane->height;
            //ROS_WARN("%d - plane: %d points. height =%.2f" ,i, plane->width * plane->height,plane_height);
            fPlaneHeight = plane_height;
           
            bool bFirstPoint = true;
            for (int j = 0; j < plane->points.size(); j++) 
            {
                pcl::PointXYZRGB p = plane->points[j];
                if(bFirstPoint == true)
                {
                    boxPlane.xMax = boxPlane.xMin = p.x;
                    boxPlane.yMax = boxPlane.yMin = p.y;
                    boxPlane.zMax = boxPlane.zMin = p.z;
                    bFirstPoint = false;
                }

                if(p.x < boxPlane.xMin) { boxPlane.xMin = p.x;}
                if(p.x > boxPlane.xMax) { boxPlane.xMax = p.x;}
                if(p.y < boxPlane.yMin) { boxPlane.yMin = p.y;}
                if(p.y > boxPlane.yMax) { boxPlane.yMax = p.y;}
                if(p.z < boxPlane.zMin) { boxPlane.zMin = p.z;}
                if(p.z > boxPlane.zMax) { boxPlane.zMax = p.z;}
            } 
            //////////////////////////////////
            //测试：将平面用框框框起来
            DrawBox(boxPlane.xMin, boxPlane.xMax, boxPlane.yMin, boxPlane.yMax, boxPlane.zMin, boxPlane.zMax, 1, 0, 1);
            //ROS_WARN("[FIND_PLANE] x= (%.2f , %.2f) y=(%.2f , %.2f) z=(%.2f , %.2f)" ,boxPlane.xMin,boxPlane.xMax,boxPlane.yMin,boxPlane.yMax,boxPlane.zMin,boxPlane.zMax);
            ///////////////////////////////////
            if(plane_height > 0.5 && plane_height < 0.85) 
            break;

            // Create the filtering object
            extract.setNegative (true);
            extract.filter (*cloud_f);
            cloud_source_ptr.swap (cloud_f);
            i++;
        }

        if (planeIndices->indices.size() == 0)
        {
            std::cout << "Could not find a plane in the scene." << std::endl;
            nTimeOut ++;
            if(nTimeOut > 10)
                nStep = STEP_DONE;
        }
        else
        {
            // Copy the points of the plane to a new cloud.
            extract.setInputCloud(cloud_source_ptr);
            extract.setIndices(planeIndices);
            extract.filter(*plane);

            // Retrieve the convex hull.(生成凸包)
            pcl::ConvexHull<pcl::PointXYZRGB> hull;
            hull.setInputCloud(plane);
            // Make sure that the resulting hull is bidimensional.
            hull.setDimension(2);
            hull.reconstruct(*convexHull);

            // Redundant check.
            if (hull.getDimension() == 2)
            {
                segmented_plane.publish(plane); //在RVIZ里显示平面
                if(nStep == STEP_FIND_OBJ )
                {
                    pcl::PointCloud<pcl::PointXYZRGB>::Ptr objects(new pcl::PointCloud<pcl::PointXYZRGB>);
                    // Prism object.(用于分割出棱柱模型内部的点集)
                    pcl::ExtractPolygonalPrismData<pcl::PointXYZRGB> prism;
                    prism.setInputCloud(cloud_source_ptr);
                    prism.setInputPlanarHull(convexHull);
                    prism.setHeightLimits(-0.30, -0.05); //height limit objects lying on the plane (物体高度范围)
                    pcl::PointIndices::Ptr objectIndices(new pcl::PointIndices);

                    //Get and show all points retrieved by the hull.
                    prism.segment(*objectIndices);
                    extract.setIndices(objectIndices);
                    extract.filter(*objects);
                    segmented_objects.publish(objects); //所有物品的点集

                    // run clustering extraction on "objects" to get several pointclouds
                    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
                    pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
                    std::vector<pcl::PointIndices> cluster_indices;
                    ec.setClusterTolerance (0.02);  //近邻搜索的搜索半径
                    ec.setMinClusterSize (500);     //单个聚类需要的最少点数目
                    ec.setMaxClusterSize (10000000);//单个聚类需要的最多点数目
                    ec.setSearchMethod (tree);      //设置点云的搜索机制
                    ec.setInputCloud (objects);     //输入物品集点云
                    ec.extract (cluster_indices);   //从点云中提取聚类

                    pcl::ExtractIndices<pcl::PointXYZRGB> extract_object_indices;//创建物品提取对象
                    std::vector<pcl::PointCloud<pcl::PointXYZRGB> > objectf;
                    cv::Mat element;

                    vObj.clear();
                    int nObjCnt = 0;
                    for(int i = 0; i<cluster_indices.size(); ++i)
                    {
                        pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
                        extract_object_indices.setInputCloud(objects);          //将物品集点云全部输入
                        extract_object_indices.setIndices(boost::make_shared<const pcl::PointIndices>(cluster_indices[i]));
                        extract_object_indices.filter(*object_cloud);           //提取输出存储到object_cloud
                        objectf.push_back(*object_cloud);

                        //统计该物品点云的空间范围
                        bool bFirstPoint = true;
                        for (int j = 0; j < object_cloud->points.size(); j++) 
                        {
                            pcl::PointXYZRGB p = object_cloud->points[j];
                            if(bFirstPoint == true)
                            {
                                boxMarker.xMax = boxMarker.xMin = p.x;
                                boxMarker.yMax = boxMarker.yMin = p.y;
                                boxMarker.zMax = boxMarker.zMin = p.z;
                                bFirstPoint = false;
                            }

                            if(p.x < boxMarker.xMin) { boxMarker.xMin = p.x;}
                            if(p.x > boxMarker.xMax) { boxMarker.xMax = p.x;}
                            if(p.y < boxMarker.yMin) { boxMarker.yMin = p.y;}
                            if(p.y > boxMarker.yMax) { boxMarker.yMax = p.y;}
                            if(p.z < boxMarker.zMin) { boxMarker.zMin = p.z;}
                            if(p.z > boxMarker.zMax) { boxMarker.zMax = p.z;}

                        }
                        if(boxMarker.xMin < 1.5 && boxMarker.yMin > -0.5 && boxMarker.yMax < 0.5)   //物品所处的空间限定
                        {
                            DrawBox(boxMarker.xMin, boxMarker.xMax, boxMarker.yMin, boxMarker.yMax, boxMarker.zMin, boxMarker.zMax, 0, 1, 0);

                            std::ostringstream stringStream;
                            stringStream << "obj_" << nObjCnt;
                            std::string obj_id = stringStream.str();
                            DrawText(obj_id,0.08, boxMarker.xMax,(boxMarker.yMin+boxMarker.yMax)/2,boxMarker.zMax + 0.04, 1,0,1);
                            nObjCnt++;
                            //ROS_WARN("[obj_%d] xMin= %.2f yMin = %.2f yMax = %.2f",i,boxMarker.xMin, boxMarker.yMin, boxMarker.yMax);
                            vObj.push_back(boxMarker);
                        } 
                    }
                    //对物品检测结果进行处理
                    if(nObjCnt > 0)
                    {
                        if(nObjDetectCounter == 0)
                        {
                            //第一帧记录最靠中间的物品
                            int nNumObj = vObj.size();
                            if(nNumObj > 0)
                            {
                                float fMidY = (vObj[0].yMin + vObj[0].yMax)/2;
                                boxLastObject = vObj[0];
                                for(int i=0;i<nNumObj;i++)
                                {
                                    float fTmpMidY = (vObj[i].yMin + vObj[i].yMax)/2;
                                    if(fabs(fTmpMidY) < fabs(fMidY))
                                    {
                                        boxLastObject = vObj[i];
                                    }
                                }
                                nObjDetectCounter ++;
                            }
                            else
                            {
                                ROS_WARN("[FIND_OBJ] nNumObj <= 0 ...");  //不会有效果
                            }
                        }
                        else
                        {
                            //后面的帧检测之前记录的物品还在不在
                            float fMidY = (boxLastObject.yMin + boxLastObject.yMax)/2;
                            int nNearIndex = 0;
                            float fNearDiff = fabs((vObj[0].yMin + vObj[0].yMax)/2 - fMidY);
                            int nNumObj = vObj.size();
                            if(nNumObj > 0)
                            {
                                for(int i=0;i<nNumObj;i++)
                                {
                                    float fTmpMidY = (vObj[i].yMin + vObj[i].yMax)/2;
                                    float fTmpDiff = fabs(fTmpMidY - fMidY);
                                    //ROS_WARN("[4_FIND_OBJ] fTmpMidY = %.2f  fTmpDiff = %.2f",fTmpMidY,fTmpDiff);
                                    if( fabs(fTmpDiff) <= fNearDiff )
                                    {
                                        boxLastObject = vObj[i];
                                        fNearDiff = fabs(fTmpDiff);
                                        if(nStep == STEP_FIND_OBJ)
                                        {
                                            nObjDetectCounter ++;
                                            ROS_WARN("[FIND_OBJ] nObjDetectCounter = %d",nObjDetectCounter);
                                        }
                                    }
                                }
                            }
                        }
                    }
                    else
                    {
                        nObjDetectCounter = 0;
                        ROS_WARN("[FIND_OBJ] nObjCnt <= 0 No object detected ...");
                        nTimeOut ++;
                        if(nTimeOut > 10)
                            nStep = STEP_DONE;
                    }
                }
                else
                {
                    //不需要检测物品时，仅仅发布平面数据即可
                    segmented_plane.publish(plane); 
                }
                

            }
            else std::cout << "The chosen hull is not planar." << std::endl;
        }
    }

    /*二、有限状态机 *************************************************************************************************/
    //1、统计识别次数，确认平面
    if(nStep == STEP_FIND_PLANE)  //(点云处理算法在上面)
    {
        mani_ctrl_msg.position[0] = 0;
        mani_ctrl_msg.position[1] = 0.16;
        mani_ctrl_pub.publish(mani_ctrl_msg);
        VelCmd(0,0,0);
        ctrl_msg.data = "pose_diff reset";
        ctrl_pub.publish(ctrl_msg);
        if(fabs(fPlaneHeight - fLastPlaneHeight) < 0.05)
        {
            nPlaneHeightCounter ++;
        }
        else
        {
            nPlaneHeightCounter = 0;
        }
        fLastPlaneHeight = fPlaneHeight;
        ROS_WARN("[FIND_PLANE] z= %.2f xm= %.2f y=(%.2f , %.2f) counter= %d" ,fPlaneHeight,boxPlane.xMin,boxPlane.yMin,boxPlane.yMax,nPlaneHeightCounter);
        if(nPlaneHeightCounter > 10)
        {
            nPlaneHeightCounter = 0;
            nTimeDelayCounter = 0;
            nStep = STEP_PLANE_DIST;
            fMoveTargetX = boxPlane.xMin - 0.7;
            fMoveTargetY = 0;
        }
        result_msg.data = "find plane";
        result_pub.publish(result_msg);
    }

    //2、前后运动控制到平面的距离
    if(nStep == STEP_PLANE_DIST && nPlaneDistMode == 1)
    {
        float fMinDist = 100;
        for (int i = 0; i < cloud_source_ptr->points.size(); i++) 
        {
            pcl::PointXYZRGB p = cloud_source_ptr->points[i];
            if(
                p.y > -0.2 && p.y < 0.2 && 
                p.z > fPlaneHeight-0.05 && p.z < fPlaneHeight+0.05 && 
                p.x < fMinDist)
            {
                fMinDist = p.x;
            }
        }
        fPlaneDist = fMinDist;
        ROS_WARN("[PLANE_DIST] dist= %.2f" ,fPlaneDist);
        float diff = fPlaneDist - fTargetPlaneDist;
        if(fabs(diff) < 0.02)
        {
            nObjDetectCounter = 0;
            nStep = STEP_FIND_OBJ;
            VelCmd(0,0,0);
        } 
        else
        {
            if(diff > 0)
            {
                //距离还太远，前进
                VelCmd(0.1,0,0);
            }
            else
            {
                //距离还太近，后退
                VelCmd(-0.1,0,0);
            }
        }

        result_msg.data = "plane dist";
        result_pub.publish(result_msg);
    }

    //3、检测物品，挑选出准备抓取的目标物品
    if(nStep == STEP_FIND_OBJ)  //(点云处理算法在上面)
    {
        VelCmd(0,0,0);
        ctrl_msg.data = "pose_diff reset";
        ctrl_pub.publish(ctrl_msg);
        if(nObjDetectCounter > 3)
        {
            nObjDetectCounter = 0;
            // 目标物品的坐标
            fObjGrabX = boxLastObject.xMin;
            fObjGrabY = (boxLastObject.yMin+boxLastObject.yMax)/2;
            fObjGrabZ = boxLastObject.zMax;
            ROS_WARN("[OBJ_TO_GRAB] x = %.2f y= %.2f ,z= %.2f w= %.2f" ,fObjGrabX, fObjGrabY, fObjGrabZ,boxLastObject.yMin - boxLastObject.yMax);

            // 判断一下物品和桌子边缘的距离，如果放得太靠里，放弃抓取以免机器人撞击桌子
            // if(fabs(fObjGrabX - fTargetPlaneDist) > 0.4 )
            // {
            //     ROS_WARN("[OBJ_TO_GRAB] Object is hard to reach !!!");
            //     nStep = STEP_DONE;
            // }
            // else
            // {
                fMoveTargetX = 0.0f;
                fMoveTargetY = fObjGrabY - fTargetGrabY + grab_y_offset;
                //ROS_WARN("[MOVE_TARGET] x = %.2f y= %.2f " ,fMoveTargetX, fMoveTargetY);
                nStep = STEP_OBJ_DIST;
            // }
        }

        result_msg.data = "find objects";
        result_pub.publish(result_msg);
    }
}

void PoseDiffCallback(const geometry_msgs::Pose2D::ConstPtr& msg)
{
    pose_diff.x = msg->x;
    pose_diff.y = msg->y;
    pose_diff.theta = msg->theta;
}

void DrawBox(float inMinX, float inMaxX, float inMinY, float inMaxY, float inMinZ, float inMaxZ, float inR, float inG, float inB)
{
    line_box.header.frame_id = "base_footprint";
    line_box.ns = "line_box";
    line_box.action = visualization_msgs::Marker::ADD;
    line_box.id = 0;
    line_box.type = visualization_msgs::Marker::LINE_LIST;
    line_box.scale.x = 0.005;
    line_box.color.r = inR;
    line_box.color.g = inG;
    line_box.color.b = inB;
    line_box.color.a = 1.0;

    geometry_msgs::Point p;
    p.z = inMinZ;
    p.x = inMinX; p.y = inMinY; line_box.points.push_back(p);
    p.x = inMinX; p.y = inMaxY; line_box.points.push_back(p);

    p.x = inMinX; p.y = inMaxY; line_box.points.push_back(p);
    p.x = inMaxX; p.y = inMaxY; line_box.points.push_back(p);

    p.x = inMaxX; p.y = inMaxY; line_box.points.push_back(p);
    p.x = inMaxX; p.y = inMinY; line_box.points.push_back(p);

    p.x = inMaxX; p.y = inMinY; line_box.points.push_back(p);
    p.x = inMinX; p.y = inMinY; line_box.points.push_back(p);

    p.z = inMaxZ;
    p.x = inMinX; p.y = inMinY; line_box.points.push_back(p);
    p.x = inMinX; p.y = inMaxY; line_box.points.push_back(p);

    p.x = inMinX; p.y = inMaxY; line_box.points.push_back(p);
    p.x = inMaxX; p.y = inMaxY; line_box.points.push_back(p);

    p.x = inMaxX; p.y = inMaxY; line_box.points.push_back(p);
    p.x = inMaxX; p.y = inMinY; line_box.points.push_back(p);

    p.x = inMaxX; p.y = inMinY; line_box.points.push_back(p);
    p.x = inMinX; p.y = inMinY; line_box.points.push_back(p);

    p.x = inMinX; p.y = inMinY; p.z = inMinZ; line_box.points.push_back(p);
    p.x = inMinX; p.y = inMinY; p.z = inMaxZ; line_box.points.push_back(p);

    p.x = inMinX; p.y = inMaxY; p.z = inMinZ; line_box.points.push_back(p);
    p.x = inMinX; p.y = inMaxY; p.z = inMaxZ; line_box.points.push_back(p);

    p.x = inMaxX; p.y = inMaxY; p.z = inMinZ; line_box.points.push_back(p);
    p.x = inMaxX; p.y = inMaxY; p.z = inMaxZ; line_box.points.push_back(p);

    p.x = inMaxX; p.y = inMinY; p.z = inMinZ; line_box.points.push_back(p);
    p.x = inMaxX; p.y = inMinY; p.z = inMaxZ; line_box.points.push_back(p);
    marker_pub.publish(line_box);
}

static int nTextNum = 2;
void DrawText(std::string inText, float inScale, float inX, float inY, float inZ, float inR, float inG, float inB)
{
    text_marker.header.frame_id = "base_footprint";
    text_marker.ns = "line_obj";
    text_marker.action = visualization_msgs::Marker::ADD;
    text_marker.id = nTextNum;
    nTextNum ++;
    text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    text_marker.scale.z = inScale;
    text_marker.color.r = inR;
    text_marker.color.g = inG;
    text_marker.color.b = inB;
    text_marker.color.a = 1.0;

    text_marker.pose.position.x = inX;
    text_marker.pose.position.y = inY;
    text_marker.pose.position.z = inZ;
    
    text_marker.pose.orientation=tf::createQuaternionMsgFromYaw(1.0);

    text_marker.text = inText;

    marker_pub.publish(text_marker);
}

void RemoveBoxes()
{
    line_box.action = 3;
    line_box.points.clear();
    marker_pub.publish(line_box);
    line_follow.action = 3;
    line_follow.points.clear();
    marker_pub.publish(line_follow);
    text_marker.action = 3;
    marker_pub.publish(text_marker);
}

void VelCmd(float inVx , float inVy, float inTz)
{
    geometry_msgs::Twist vel_cmd;
    vel_cmd.linear.x = inVx;
    vel_cmd.linear.y = inVy;
    vel_cmd.angular.z = inTz;
    vel_pub.publish(vel_cmd);
}

void BehaviorCB(const std_msgs::String::ConstPtr &msg)
{
    int nFindIndex = 0;
    nFindIndex = msg->data.find("grab start");
    if( nFindIndex >= 0 )
    {
        nTimeOut = 0;
        VelCmd(0,0,0);
        nStep = STEP_FIND_PLANE;
        ROS_WARN("[grab_start] ");
    }

    nFindIndex = msg->data.find("grab stop");
    if( nFindIndex >= 0 )
    {
        ROS_WARN("[grab_stop] ");
        nTimeOut = 0;
        nStep = STEP_WAIT;
        geometry_msgs::Twist vel_cmd;
        vel_cmd.linear.x = 0;
        vel_cmd.linear.y = 0;
        vel_cmd.linear.z = 0;
        vel_cmd.angular.x = 0;
        vel_cmd.angular.y = 0;
        vel_cmd.angular.z = 0;
        vel_pub.publish(vel_cmd);
    }

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "wpb_home_grab_server");
    ROS_INFO("wpb_home_grab_server");
    tf_listener = new tf::TransformListener(); 

    ros::NodeHandle nh_param("~");
    nh_param.param<std::string>("topic", pc_topic, "/kinect2/sd/points");
    bool start_flag = false;
    nh_param.param<bool>("start", start_flag, false);
    if(start_flag == true)
    {
        nStep = STEP_FIND_PLANE;
    }

    ros::NodeHandle nh;
    ros::Subscriber pc_sub = nh.subscribe(pc_topic, 10 , ProcCloudCB);

    pc_pub = nh.advertise<sensor_msgs::PointCloud2>("obj_pointcloud",1);
    marker_pub = nh.advertise<visualization_msgs::Marker>("obj_marker", 10);

    segmented_objects = nh.advertise<PointCloud> ("segmented_objects",1);
    segmented_plane = nh.advertise<PointCloud> ("segmented_plane",1);
    vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 30);
    mani_ctrl_pub = nh.advertise<sensor_msgs::JointState>("/wpb_home/mani_ctrl", 30);
    result_pub = nh.advertise<std_msgs::String>("/wpb_home/grab_result", 30);

    ros::Subscriber sub_sr = nh.subscribe("/wpb_home/behaviors", 30, BehaviorCB);
    ctrl_pub = nh.advertise<std_msgs::String>("/wpb_home/ctrl", 30);
    ros::Subscriber pose_diff_sub = nh.subscribe("/wpb_home/pose_diff", 1, PoseDiffCallback);

    mani_ctrl_msg.name.resize(2);
    mani_ctrl_msg.position.resize(2);
    mani_ctrl_msg.velocity.resize(2);
    mani_ctrl_msg.name[0] = "lift";
    mani_ctrl_msg.name[1] = "gripper";
    mani_ctrl_msg.position[0] = 0;
    mani_ctrl_msg.velocity[0] = 0.5;     //升降速度(单位:米/秒)
    mani_ctrl_msg.position[1] = 0.16;
    mani_ctrl_msg.velocity[1] = 5;       //手爪开合角速度(单位:度/秒)

    // nh_param.getParam("grab/grab_y_offset", grab_y_offset);
    // nh_param.getParam("grab/grab_lift_offset", grab_lift_offset);
    // nh_param.getParam("grab/grab_forward_offset", grab_forward_offset);
    // nh_param.getParam("grab/grab_gripper_value", grab_gripper_value);

    bool bActive = false;
    nh_param.param<bool>("start", bActive, false);
    if(bActive == true)
    {
        VelCmd(0,0,0);
        nStep = STEP_FIND_PLANE;
    }

    ros::Rate r(30);
    while(nh.ok())
    {
        //2、前后运动控制到平面的距离
        if(nStep == STEP_PLANE_DIST && nPlaneDistMode == 2)
        {
            float vx,vy;
            vx = (fMoveTargetX - pose_diff.x)/2;
            vy = (fMoveTargetY - pose_diff.y)/2;

            VelCmd(vx,vy,0);

            //ROS_INFO("[MOVE] T(%.2f %.2f)  od(%.2f , %.2f) v(%.2f,%.2f)" ,fMoveTargetX, fMoveTargetY, pose_diff.x ,pose_diff.y,vx,vy);

            if(fabs(vx) < 0.01 && fabs(vy) < 0.01)
            {
                VelCmd(0,0,0);
                ctrl_msg.data = "pose_diff reset";
                ctrl_pub.publish(ctrl_msg);
                nStep = STEP_FIND_OBJ;
            }

            result_msg.data = "plane dist";
            result_pub.publish(result_msg);
        }
    
        //4、左右平移对准目标物品 
        if(nStep == STEP_OBJ_DIST)
        {
            float vx,vy;
            vx = (fMoveTargetX - pose_diff.x)/2;
            vy = (fMoveTargetY - pose_diff.y)/2;

            VelCmd(vx,vy,0);
            //ROS_INFO("[MOVE] T(%.2f %.2f)  od(%.2f , %.2f) v(%.2f,%.2f)" ,fMoveTargetX, fMoveTargetY, pose_diff.x ,pose_diff.y,vx,vy);

            if(fabs(vx) < 0.01 && fabs(vy) < 0.01)
            {
                VelCmd(0,0,0);
                ctrl_msg.data = "pose_diff reset";
                ctrl_pub.publish(ctrl_msg);
                nStep = STEP_HAND_UP;
                nTimeDelayCounter = 0;
            }

            result_msg.data = "object x";
            result_pub.publish(result_msg);
        }

        //5、抬起手臂
        if(nStep == STEP_HAND_UP)
        {
            if(nTimeDelayCounter == 0)
            {
                mani_ctrl_msg.position[0] = fPlaneHeight + grab_lift_offset;
                mani_ctrl_msg.position[1] = 0.16;
                mani_ctrl_pub.publish(mani_ctrl_msg);
                ROS_WARN("[MANI_CTRL] lift= %.2f  gripper= %.2f " ,mani_ctrl_msg.position[0], mani_ctrl_msg.position[1]);
                result_msg.data = "hand up";
                result_pub.publish(result_msg);
            }
            nTimeDelayCounter ++;
            VelCmd(0,0,0);
            if(nTimeDelayCounter > 15*30)
            {
                fMoveTargetX = fObjGrabX -0.55 + grab_forward_offset;
                fMoveTargetY = 0;
                ROS_WARN("[STEP_FORWARD] x = %.2f y= %.2f " ,fMoveTargetX, fMoveTargetY);
                nTimeDelayCounter = 0;
                nStep = STEP_FORWARD;
            }
        }
    
         //6、前进靠近物品
        if(nStep == STEP_FORWARD)
        {
            float vx,vy;
            vx = (fMoveTargetX - pose_diff.x)/2;
            vy = (fMoveTargetY - pose_diff.y)/2;

            VelCmd(vx,vy,0);

            //ROS_INFO("[MOVE] T(%.2f %.2f)  od(%.2f , %.2f) v(%.2f,%.2f)" ,fMoveTargetX, fMoveTargetY, pose_diff.x ,pose_diff.y,vx,vy);

            if(fabs(vx) < 0.01 && fabs(vy) < 0.01)
            {
                VelCmd(0,0,0);
                ctrl_msg.data = "pose_diff reset";
                ctrl_pub.publish(ctrl_msg);
                nStep = STEP_GRAB;
            }

            result_msg.data = "forward";
            result_pub.publish(result_msg);
        }

        //7、抓取物品
        if(nStep == STEP_GRAB)
        {
            if(nTimeDelayCounter == 0)
            {
                result_msg.data = "grab";
                result_pub.publish(result_msg);
            }
            mani_ctrl_msg.position[1] = grab_gripper_value;      //抓取物品手爪闭合宽度
            mani_ctrl_pub.publish(mani_ctrl_msg);
            //ROS_WARN("[MANI_CTRL] lift= %.2f  gripper= %.2f " ,mani_ctrl_msg.position[0], mani_ctrl_msg.position[1]);

            nTimeDelayCounter++;
            VelCmd(0,0,0);
            if(nTimeDelayCounter > 8*30)
            {
                nTimeDelayCounter = 0;
                nStep = STEP_OBJ_UP;
            }
        }

        //8、拿起物品
        if(nStep == STEP_OBJ_UP)
        {
            if(nTimeDelayCounter == 0)
            {
                mani_ctrl_msg.position[0] += 0.03;
                mani_ctrl_pub.publish(mani_ctrl_msg);
                //ROS_WARN("[MANI_CTRL] lift= %.2f  gripper= %.2f " ,mani_ctrl_msg.position[0], mani_ctrl_msg.position[1]);
                result_msg.data = "object up";
                result_pub.publish(result_msg);
            }
            nTimeDelayCounter++;
            VelCmd(0,0,0);
            if(nTimeDelayCounter > 3*30)
            {
                fMoveTargetX = -(fTargetGrabX-0.4);
                fMoveTargetY = 0;
                //ROS_WARN("[STEP_BACKWARD] x= %.2f y= %.2f " ,fMoveTargetX, fMoveTargetY);

                nTimeDelayCounter = 0;
                nStep = STEP_BACKWARD;
            }
        }

        //9、带着物品后退
        if(nStep == STEP_BACKWARD)
        {
            //ROS_WARN("[STEP_BACKWARD] nTimeDelayCounter = %d " ,nTimeDelayCounter);
            //nTimeDelayCounter++;
            
            float vx,vy;
            vx = (fMoveTargetX - pose_diff.x)/2;
            vy = (fMoveTargetY - pose_diff.y)/2;

            VelCmd(vx,vy,0);

            //ROS_INFO("[MOVE] T(%.2f %.2f)  od(%.2f , %.2f) v(%.2f,%.2f)" ,fMoveTargetX, fMoveTargetY, pose_diff.x ,pose_diff.y,vx,vy);

            if(fabs(vx) < 0.01 && fabs(vy) < 0.01)
            {
                VelCmd(0,0,0);
                ctrl_msg.data = "pose_diff reset";
                ctrl_pub.publish(ctrl_msg);
                nStep = STEP_DONE;
            }

            result_msg.data = "backward";
            result_pub.publish(result_msg);
        }

        //10、抓取任务完毕
        if(nStep == STEP_DONE)
        {
            result_msg.data = "done";
            result_pub.publish(result_msg);
            nTimeOut = 0;
            nStep = STEP_WAIT;
        }

        ros::spinOnce();
        r.sleep();
    }

    delete tf_listener; 

    return 0;

}

<|start_filename|>robot/src/drivers/waterplus_map_tools/src/network/UDPServer.c<|end_filename|>
/*!
 @header     UDPServer.c
 @abstract   UDP通讯服务
 @author     张万杰
 */
#include "UDPServer.h"
#include "UDPClient.h"

static char buffer[1024];
static int sockfd,len;
static struct sockaddr_in addr;
static int addr_len;
static pthread_t hThread;

static int bFrameStart = 0;
static int nParseIndex = 0;
static int nLenToParse = 10;
static unsigned char m_byteLast = 0;
static unsigned char arParseBuf[1024];

static bool bNewCmd = false;
static ST_Ctrl ctrl_recv;

int BytesToWord(unsigned char* inBuf)
{
    int res = 0;
    res = inBuf[0];
    res <<= 8;
    res |= inBuf[1];
    return res;
}

float BytesToFloat(unsigned char* inBuf)
{
    float res = 0;
    memcpy((char*)&res,inBuf,sizeof(float));
    return res;
}

void UDP_Server_ParseFrame(unsigned char* inBuf,int inLen)
{
    printf("%d [UDP_Recv] ",inLen);
    int i =0;
    for(i=0;i<inLen;i++)
    {
        printf("%.2X ",inBuf[i]);
    }
    printf("\n");

    ctrl_recv.ctrl = inBuf[2];
    if(ctrl_recv.ctrl == CTRL_MOVETO_POS)
    {
        ctrl_recv.x = BytesToFloat(&(inBuf[3]));
        ctrl_recv.y = BytesToFloat(&(inBuf[7]));
        ctrl_recv.angle = BytesToFloat(&(inBuf[11]));
        printf("[CTRL_MOVETO_POS] ( %.2f , %.2f ) - %.2f\n",ctrl_recv.x,ctrl_recv.y,ctrl_recv.angle);
    }
    if(ctrl_recv.ctrl == CTRL_MOVETO_NAME)
    {
        ctrl_recv.wp_name = inBuf[3];
        printf("[CTRL_MOVETO_NAME]  - \'%c\'\n",ctrl_recv.wp_name);
    }

    bNewCmd = true;
   
}

void UDP_Server_Parse(unsigned char inChar)
{
    if(bFrameStart == 0)
    {
        if(m_byteLast == 0x55 && inChar == 0xaa)
        {
            bFrameStart = 1;
            arParseBuf[0] = 0x55;
            arParseBuf[1] = 0xaa;
            nParseIndex = 2;
        }
        else
        {
            m_byteLast = inChar;
        }
    }
    else
    {
        //开始缓存
        if(nParseIndex == 2)
        {
            if(inChar == CTRL_STOP)
            {
                nLenToParse = 3;
            }
            if(inChar == CTRL_MOVETO_POS)
            {
                nLenToParse = 15;
            }
            if(inChar == CTRL_MOVETO_NAME)
            {
                nLenToParse = 4;
            }
        }
        arParseBuf[nParseIndex] = inChar;
        nParseIndex ++;

        if(nParseIndex >= nLenToParse)
        {
            UDP_Server_ParseFrame((unsigned char*)arParseBuf,nLenToParse);
            bFrameStart = 0;
            nParseIndex = 0;
            m_byteLast = 0;
        }
    }
}

void *threadUDPServer_func()
{
    printf ("[UDP_Server]threadUDPServer_func start...\n");
    while(1)
	{
        bzero(buffer,sizeof(buffer));
        len = recvfrom(sockfd,buffer,sizeof(buffer), 0 , (struct sockaddr *)&addr ,&addr_len);
        /*显示client端的网络地址*/
        printf("receive %d from %s\n" , len , inet_ntoa( addr.sin_addr));

        int i = 0;
        for(i=0;i<len;i++)
        {
            UDP_Server_Parse(buffer[i]);
        }

        /*初始化udp_client端**/
        //InitUDPClient(inet_ntoa( addr.sin_addr),10010);

   }
    printf("[UDP_Server]threadUDPServer_func exit\n");
    pthread_exit(NULL);
}

void InitUDPServer(int inPort)
{
	printf("[UDP_Server]InitUDPServer( port = %d )...\n",inPort);

    addr_len = sizeof(struct sockaddr_in);
    /*建立socket*/
    if((sockfd=socket(AF_INET,SOCK_DGRAM,0))<0)
    {
        perror ("socket");
        return;
    }
    /*填写sockaddr_in 结构*/
    bzero ( &addr, sizeof(addr) );
    addr.sin_family=AF_INET;
    addr.sin_port=htons(inPort);
    addr.sin_addr.s_addr=htonl(INADDR_ANY) ;
    if (bind(sockfd, (struct sockaddr *)&addr, sizeof(addr))<0)
    {
        perror("connect");
        return;
    }

    int res;
    res = pthread_create(&hThread, NULL, threadUDPServer_func, NULL);
}

int UDPServerLoop()
{
    unsigned char ip[10] = "hello";

    while(1)
    {
        bzero(buffer,sizeof(buffer));
        len = recvfrom(sockfd,buffer,sizeof(buffer), 0 , (struct sockaddr *)&addr ,&addr_len);
        /*显示client端的网络地址*/
        printf("receive %d from %s\n" , len , inet_ntoa( addr.sin_addr));
        /*将字串返回给client端**/
        //InitUDPClient(inet_ntoa( addr.sin_addr),20180);
        //UDPClientSend(ip,10);

        usleep(1000*10);
   }
}


bool GetCtrlCmd(ST_Ctrl* inCtrl)
{

    if(bNewCmd == true)
    {
        memcpy(inCtrl,&ctrl_recv,sizeof(ctrl_recv));
        bNewCmd = false;
        return true;
    }
    else
    {
        return false;
    }
}


<|start_filename|>robot/src/basic/src/basic_move.cpp<|end_filename|>
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "basic/moveMsg.h"
#include "basic/checksrv.h"
#include <iostream>
#include <algorithm>

using namespace std;

float LINEAR_MAX = 0.6;
float ANGULAR_MAX = 1.0;
float LINEAR_MIN = -0.6;
float ANGULAR_MIN = -1.0;
int MODE = 1;

ros::Publisher pub;
ros::ServiceClient check;
geometry_msgs::Twist vel;

float checkLinear(float speed){
    if (speed > 0){
        return min(speed,LINEAR_MAX);
    }else{
        return max(speed, LINEAR_MIN);
    }
}

float checkAngular(float speed){
    if (speed > 0){
        return min(speed,ANGULAR_MAX);
    }else{
        return max(speed, ANGULAR_MIN);
    }
}


void move_listen(const basic::moveMsg::ConstPtr& msg){
    if (vel.linear.x != msg->x || vel.linear.y != msg->y || vel.angular.z != msg->z){
        ROS_INFO("x: %.6f, y: %.6f, z: %.6f\n", msg->x, msg->y, msg->z);
    }
    vel.linear.x = checkLinear(msg->x);
    vel.linear.y = checkLinear(msg->y);
    vel.linear.z = 0;
    vel.angular.x = 0;
    vel.angular.y = 0;
    vel.angular.z = checkLinear(msg->z);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "basic_move");//the name 
    ros::NodeHandle nh;

    pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    check = nh.serviceClient<basic::checksrv>("/fight_with_hair/basic_move/avoid");
    ros::Subscriber sub = nh.subscribe("/fight_with_hair/basic_move/vel", 10, move_listen);

    ros::Rate loop_rate(10);
    while(ros::ok()) {
        ros::spinOnce();
        if (MODE == 1){
            basic::checksrv checkmsg;
            checkmsg.request.x = vel.linear.x;
            checkmsg.request.y = vel.linear.y;
            ROS_INFO("x: %.3f; y: %.3f",checkmsg.request.x,checkmsg.request.y);
            check.call(checkmsg);
            vel.linear.x = checkmsg.response.x_new;
            vel.linear.y = checkmsg.response.y_new;
            ROS_INFO("new x: %.3f; new y: %.3f",checkmsg.response.x_new,checkmsg.response.y_new);
        }
        pub.publish(vel);
        loop_rate.sleep();
    }
    return 0;
}


<|start_filename|>robot/src/navi/test/test_navi_adaptor.cpp<|end_filename|>
#include <iostream>
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <stdlib.h>
#include <time.h>

#include "geometry_msgs/Twist.h"
#include "basic/moveMsg.h"

using namespace std;

double LINEAR_VEL_MAX = 0.8;
double ANGULAR_VEL_MAX = 0.7;
basic::moveMsg new_move_msg;

double set_new_vel(double cur_vel, bool is_ang)
{
    double vel_lim = is_ang ? ANGULAR_VEL_MAX : LINEAR_VEL_MAX;
    if (cur_vel < 0)
    {
        return max(cur_vel, -vel_lim);
    }
    else if (cur_vel > 0)
    {
        return min(cur_vel, vel_lim);
    }
    return 0;
}

double randf(double x, double y)
{
    return x + 1.0 * rand() / RAND_MAX * (y - x);
}

double gene_line()
{
    return randf(-LINEAR_VEL_MAX * 2, LINEAR_VEL_MAX * 2);
}

double gene_angle()
{
    return randf(-ANGULAR_VEL_MAX * 2, ANGULAR_VEL_MAX * 2);
}

void adapt(const geometry_msgs::Twist &vel)
{
    new_move_msg.x = set_new_vel(vel.linear.x, false);
    new_move_msg.y = set_new_vel(vel.linear.y, false);
    new_move_msg.z = set_new_vel(vel.angular.z, true);
}

TEST(adapt_test, test1)
{
    geometry_msgs::Twist vel;
    vel.linear.x = 0;
    vel.linear.y = 0;
    vel.angular.z = 0;
    adapt(vel);
    EXPECT_FLOAT_EQ(new_move_msg.x, 0);
    EXPECT_FLOAT_EQ(new_move_msg.y, 0);
    EXPECT_FLOAT_EQ(new_move_msg.z, 0);
}

TEST(adapt_test, test2)
{
    geometry_msgs::Twist vel;
    vel.linear.x = 0;
    vel.linear.y = 0;
    vel.angular.z = 0.7;
    adapt(vel);
    EXPECT_FLOAT_EQ(new_move_msg.x, 0);
    EXPECT_FLOAT_EQ(new_move_msg.y, 0);
    EXPECT_FLOAT_EQ(new_move_msg.z, 0.7);
}

TEST(adapt_test, test3)
{
    geometry_msgs::Twist vel;
    vel.linear.x = 0.8;
    vel.linear.y = 0.8;
    vel.angular.z = 0.7;
    adapt(vel);
    EXPECT_FLOAT_EQ(new_move_msg.x, 0.8);
    EXPECT_FLOAT_EQ(new_move_msg.y, 0.8);
    EXPECT_FLOAT_EQ(new_move_msg.z, 0.7);
}

TEST(adapt_test, test4)
{
    srand(time(0));
    for (int i = 0; i < 100; i++)
    {
        geometry_msgs::Twist vel;
        vel.linear.x = gene_line();
        vel.linear.y = gene_line();
        vel.angular.z = gene_angle();
        adapt(vel);
        double x, y, z;
        if (vel.linear.x <= 0)
        {
            x = max(vel.linear.x, -LINEAR_VEL_MAX);
        }
        else
        {
            x = min(vel.linear.x, LINEAR_VEL_MAX);
        }
        if (vel.linear.y <= 0)
        {
            y = max(vel.linear.y, -LINEAR_VEL_MAX);
        }
        else
        {
            y = min(vel.linear.y, LINEAR_VEL_MAX);
        }
        if (vel.angular.z <= 0)
        {
            z = max(vel.angular.z, -ANGULAR_VEL_MAX);
        }
        else
        {
            z = min(vel.angular.z, ANGULAR_VEL_MAX);
        }
        EXPECT_FLOAT_EQ(new_move_msg.x, x);
        EXPECT_FLOAT_EQ(new_move_msg.y, y);
        EXPECT_FLOAT_EQ(new_move_msg.z, z);
    }
}

int main(int argc, char **argv)
{
    srand(time(0));
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}

<|start_filename|>robot/src/drivers/xfyun_waterplus/src/wav_rate_node.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* @author <NAME>                                             */

#include "ros/ros.h"
#include <sstream>
#include <string.h>
#include <stdlib.h>
#include "std_msgs/String.h"

static ros::Publisher reco_pub;

void sndFileCallback(const std_msgs::String::ConstPtr& msg)
{
	ROS_INFO("ffmpeg: [%s]", msg->data.c_str());
	
	char const* home_dir = getenv("HOME");
	std::string strHomeDir = home_dir;
	std::string strFF = strHomeDir + "/catkin_ws/src/xfyun_kinetic/tools/ffmpeg -i ";

	std::stringstream ss;
	ss << strFF << msg->data << " -v 0 -map_channel 0.0.0 -ar 16k -y /dev/shm/tmp.wav"; //16khz,单声道
	system(ss.str().c_str());

	std_msgs::String reco_msg;
	reco_msg.data = "/dev/shm/tmp.wav";
	reco_pub.publish(reco_msg);
}

int main(int argc, char *argv[])
{
	ros::init(argc, argv, "wav_rate_node");

	ros::NodeHandle n;
	reco_pub = n.advertise<std_msgs::String>("/xfyun/reco_file", 2);
	ros::Subscriber sub = n.subscribe("/xfyun/snd_file", 2, sndFileCallback);

	ros::spin();

	return 0;
}

<|start_filename|>robot/src/drivers/wpb_home_apps/src/fwh_s_test.cpp<|end_filename|>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include "f_fwh_script.h"

static FwhScript fwh_script;

void KeywordCB(const std_msgs::String::ConstPtr & msg)
{
    //ROS_WARN("[inno_script_KeywordCB] - %s",msg->data.c_str());
    string strListen = msg->data;
    fwh_script.strListen = strListen;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "fwh_script");
    ROS_INFO("[main] fwh_script");
    fwh_script.Init();
    fwh_script.Queue();
    fwh_script.ShowActs();

    ros::NodeHandle n;
    ros::Subscriber sub_sr = n.subscribe("/xfyun/iat", 10, KeywordCB);
    ros::Rate r(10);
    ros::spinOnce();
    while(ros::ok())
    {
        fwh_script.Main();
        ros::spinOnce();
        r.sleep();
    }

    return 0;
}

<|start_filename|>robot/src/drivers/lidar/src/lidar_data_node.cpp<|end_filename|>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/LaserScan.h>

void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    int nNum = scan->ranges.size();
    for(int i=0 ; i<nNum ; i++)
    {
        ROS_INFO("Point[%d] = %f", i, scan->ranges[i]); 
    }
}

int main(int argc, char** argv)
{
    ros::init(argc,argv,"wpb_home_lidar_data");
    
    ROS_INFO("wpb_home_lidar_data start!");

    ros::NodeHandle nh;
    ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &lidarCallback);

    ros::spin();
}

<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/spinslider.h<|end_filename|>
#ifndef SPINSLIDER_H
#define SPINSLIDER_H

#include <QWidget>
#include "cctrldashboard.h"

namespace Ui {
class spinslider;
}

class spinslider : public QWidget
{
  Q_OBJECT

public:
  explicit spinslider(QWidget *parent = 0);
  ~spinslider();

private:
  Ui::spinslider *ui;
  CCtrlDashBoard *DashBoard_x;
};

#endif // SPINSLIDER_H


<|start_filename|>robot/src/drivers/waterplus_map_tools/include/add_charger_tool.h<|end_filename|>
#ifndef ADD_CHARGER_TOOL_H
#define ADD_CHARGER_TOOL_H

#ifndef Q_MOC_RUN
#include <QObject>
#include <ros/ros.h>
#include "rviz/default_plugin/tools/pose_tool.h"
#endif

namespace rviz
{
    class Arrow;
    class DisplayContext;
    class StringProperty;

    class AddChargerTool: public rviz::PoseTool
    {
        Q_OBJECT
        public:
        AddChargerTool();
        ~AddChargerTool();

        virtual void onInitialize();

        protected:
        virtual void onPoseSet(double x, double y, double theta);

        private Q_SLOTS:
        void updateTopic();

        private:
        ros::NodeHandle nh_;
        ros::Publisher pub_;
        ros::ServiceClient cliGetChName;
        StringProperty* topic_property_;
    };

}

#endif // ADD_CHARGER_TOOL_H

<|start_filename|>robot/src/drivers/wpb_home/wpb_home_behaviors/src/wpb_home_place_action.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*!******************************************************************
 @author     ZhangWanjie
 ********************************************************************/

#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/String.h>
#include <tf/transform_broadcaster.h>

// 放置参数调节（单位：米）
static float place_dist = 0.9;               //机器人放置前和目标点距离
static float place_y_offset = 0.0f;          //机器人的横向位移补偿量
static float place_lift_offset = 0.0f;       //手臂抬起高度的补偿量
static float place_forward_offset = 0.0f;    //手臂抬起后，机器人向前移动的位移补偿量
static float place_gripper_value = 0.15;     //放置物品时，手爪松开后的手指间距

#define STEP_WAIT           0
#define STEP_PLACE_DIST     1
#define STEP_FORWARD        2
#define STEP_RELEASE        3
#define STEP_BACKWARD       4
#define STEP_Y_BACK         5
#define STEP_DONE           6
static int nStep = STEP_WAIT;

static std::string pc_topic;
static ros::Publisher vel_pub;
static ros::Publisher mani_ctrl_pub;
static sensor_msgs::JointState mani_ctrl_msg;
static ros::Publisher result_pub;
static std_msgs::String result_msg;
static ros::Publisher odom_ctrl_pub;
static std_msgs::String odom_ctrl_msg;
static geometry_msgs::Pose2D pose_diff;

void VelCmd(float inVx , float inVy, float inTz);

static float fPlaceX = 0;
static float fPlaceY = 0;
static float fPlaceZ = 0;
static float fMoveTargetX = 0;
static float fMoveTargetY = 0;

static int nTimeDelayCounter = 0;

void PlaceActionCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
    // 放置物品的坐标
    fPlaceX = msg->position.x;
    fPlaceY = msg->position.y;
    fPlaceZ = msg->position.z;
    ROS_WARN("[Place] x = %.2f y= %.2f ,z= %.2f " ,fPlaceX, fPlaceY, fPlaceZ);
    odom_ctrl_msg.data = "pose_diff reset";
    odom_ctrl_pub.publish(odom_ctrl_msg);

    // ajudge the dist to place
    fMoveTargetX = fPlaceX - place_dist;
    fMoveTargetY = fPlaceY + place_y_offset;
    ROS_WARN("[MOVE_TARGET] x = %.2f y= %.2f " ,fMoveTargetX, fMoveTargetY);
    nTimeDelayCounter = 0;
    nStep = STEP_PLACE_DIST;
}

void PoseDiffCallback(const geometry_msgs::Pose2D::ConstPtr& msg)
{
    pose_diff.x = msg->x;
    pose_diff.y = msg->y;
    pose_diff.theta = msg->theta;
}

void VelCmd(float inVx , float inVy, float inTz)
{
    geometry_msgs::Twist vel_cmd;
    vel_cmd.linear.x = inVx;
    vel_cmd.linear.y = inVy;
    vel_cmd.angular.z = inTz;
    vel_pub.publish(vel_cmd);
}

void BehaviorCB(const std_msgs::String::ConstPtr &msg)
{
    int nFindIndex = msg->data.find("place stop");
    if( nFindIndex >= 0 )
    {
        ROS_WARN("[place stop] ");
        nStep = STEP_WAIT;
        geometry_msgs::Twist vel_cmd;
        vel_cmd.linear.x = 0;
        vel_cmd.linear.y = 0;
        vel_cmd.linear.z = 0;
        vel_cmd.angular.x = 0;
        vel_cmd.angular.y = 0;
        vel_cmd.angular.z = 0;
        vel_pub.publish(vel_cmd);
    }

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "wpb_home_place_action");
    ROS_INFO("wpb_home_place_action");

    ros::NodeHandle nh;

    vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 30);
    mani_ctrl_pub = nh.advertise<sensor_msgs::JointState>("/wpb_home/mani_ctrl", 30);
    result_pub = nh.advertise<std_msgs::String>("/wpb_home/place_result", 30);

    ros::Subscriber sub_grab_pose = nh.subscribe("/wpb_home/place_action", 1, PlaceActionCallback);
    ros::Subscriber sub_beh = nh.subscribe("/wpb_home/behaviors", 30, BehaviorCB);
    odom_ctrl_pub = nh.advertise<std_msgs::String>("/wpb_home/ctrl", 30);
    ros::Subscriber pose_diff_sub = nh.subscribe("/wpb_home/pose_diff", 1, PoseDiffCallback);

    mani_ctrl_msg.name.resize(2);
    mani_ctrl_msg.position.resize(2);
    mani_ctrl_msg.velocity.resize(2);
    mani_ctrl_msg.name[0] = "lift";
    mani_ctrl_msg.name[1] = "gripper";
    mani_ctrl_msg.position[0] = 0;
    mani_ctrl_msg.velocity[0] = 0.5;     //升降速度(单位:米/秒)
    mani_ctrl_msg.position[1] = 0.032;
    mani_ctrl_msg.velocity[1] = 5;       //手爪开合角速度(单位:度/秒)

    ros::Rate r(30);
    while(nh.ok())
    {
        //1、左右平移对准放置目标点，同时抬起手臂 
        if(nStep == STEP_PLACE_DIST)
        {
            float vx,vy;
            vx = (fMoveTargetX - pose_diff.x)/2;
            vy = (fMoveTargetY - pose_diff.y)/2;
            //ROS_INFO("[MOVE] T(%.2f %.2f)  od(%.2f , %.2f) v(%.2f,%.2f)" ,fMoveTargetX, fMoveTargetY, pose_diff.x ,pose_diff.y,vx,vy);
            if(fabs(vx) < 0.01 && fabs(vy) < 0.01)
            {
                VelCmd(0,0,0);
                if(nTimeDelayCounter > 8*30)
                {
                    odom_ctrl_msg.data = "pose_diff reset";
                    odom_ctrl_pub.publish(odom_ctrl_msg);
                    fMoveTargetX = place_dist - 0.65 + place_forward_offset;
                    fMoveTargetY = 0;
                    nTimeDelayCounter = 0;
                    nStep = STEP_FORWARD;
                    continue;
                }
            }
            else
            {
                VelCmd(vx,vy,0);
            }

            mani_ctrl_msg.position[0] = fPlaceZ + 0.03 + place_lift_offset;
            mani_ctrl_pub.publish(mani_ctrl_msg);
            nTimeDelayCounter ++;

            result_msg.data = "dist to place";
            result_pub.publish(result_msg);
        }

        //2、前进靠近放置点
        if(nStep == STEP_FORWARD)
        {
            float vx,vy;
            vx = (fMoveTargetX - pose_diff.x)/2;
            vy = (fMoveTargetY - pose_diff.y)/2;

            VelCmd(vx,vy,0);
            //ROS_INFO("[STEP_FORWARD] T(%.2f %.2f)  od(%.2f , %.2f) v(%.2f,%.2f)" ,fMoveTargetX, fMoveTargetY, pose_diff.x ,pose_diff.y,vx,vy);

            if(fabs(vx) < 0.01 && fabs(vy) < 0.01)
            {
                VelCmd(0,0,0);
                odom_ctrl_msg.data = "pose_diff reset";
                odom_ctrl_pub.publish(odom_ctrl_msg);
                nTimeDelayCounter = 0;
                ROS_WARN("[STEP_RELEASE] grab_gripper_value = %.2f",place_gripper_value);
                nStep = STEP_RELEASE;
            }

            result_msg.data = "forward";
            result_pub.publish(result_msg);
        }

        //3、释放物品
        if(nStep == STEP_RELEASE)
        {
            if(nTimeDelayCounter == 0)
            {
                result_msg.data = "release";
                result_pub.publish(result_msg);
            }
            mani_ctrl_msg.position[1] = place_gripper_value;      //释放物品手爪闭合宽度
            mani_ctrl_pub.publish(mani_ctrl_msg);
            //ROS_WARN("[STEP_RELEASE] lift= %.2f  gripper= %.2f " ,mani_ctrl_msg.position[0], mani_ctrl_msg.position[1]);

            nTimeDelayCounter++;
            VelCmd(0,0,0);
            if(nTimeDelayCounter > 5*30)
            {
                nTimeDelayCounter = 0;
                fMoveTargetX = -(fPlaceX - 0.65 + place_forward_offset);
                //fMoveTargetY = 0;
                fMoveTargetY = -(fPlaceY + place_y_offset);
                ROS_WARN("[STEP_BACKWARD] x= %.2f y= %.2f " ,fMoveTargetX, fMoveTargetY);
                nStep = STEP_BACKWARD;
            }
        }

        //4、后退
        if(nStep == STEP_BACKWARD)
        {
            //ROS_WARN("[STEP_BACKWARD] nTimeDelayCounter = %d " ,nTimeDelayCounter);
            //nTimeDelayCounter++;
            float vx,vy;
            vx = (fMoveTargetX - pose_diff.x)/2;
            if(fabs(fMoveTargetX - pose_diff.x) > 0.1) //距离小于0.25(0.9-0.65）时就已经安全了
            {
                vy = 0;
            }
            else
            {
                vy = (fMoveTargetY - pose_diff.y)/2;
                mani_ctrl_msg.position[0] = 0.5;
                mani_ctrl_pub.publish(mani_ctrl_msg);
                nTimeDelayCounter ++;
            }
            VelCmd(vx,vy,0);
            //ROS_INFO("[MOVE] T(%.2f %.2f)  od(%.2f , %.2f) v(%.2f,%.2f)" ,fMoveTargetX, fMoveTargetY, pose_diff.x ,pose_diff.y,vx,vy);
            if(fabs(vx) < 0.01 && fabs(vy) < 0.01 && nTimeDelayCounter > 8*30)
            {
                VelCmd(0,0,0);
                odom_ctrl_msg.data = "pose_diff reset";
                odom_ctrl_pub.publish(odom_ctrl_msg);
                nTimeDelayCounter = 0;
                ROS_WARN("[STEP_DONE]");
                nStep = STEP_DONE;
                // odom_ctrl_msg.data = "pose_diff reset";
                // odom_ctrl_pub.publish(odom_ctrl_msg);
                // nTimeDelayCounter = 0;
                // fMoveTargetX = 0;
                // fMoveTargetY = -(fPlaceY + place_y_offset);
                // ROS_WARN("[STEP_Y_BACK] x= %.2f y= %.2f " ,fMoveTargetX, fMoveTargetY);
                // nStep = STEP_Y_BACK;
            }

            result_msg.data = "backward";
            result_pub.publish(result_msg);
        }

        //5、横向归位
        // if(nStep == STEP_Y_BACK)
        // {
        //     //ROS_WARN("[STEP_Y_BACK] nTimeDelayCounter = %d " ,nTimeDelayCounter);
        //     //nTimeDelayCounter++;
        //     float vx,vy;
        //     vx = (fMoveTargetX - pose_diff.x)/2;
        //     vy = (fMoveTargetY - pose_diff.y)/2;
        //     VelCmd(vx,vy,0);
        //     //ROS_INFO("[MOVE] T(%.2f %.2f)  od(%.2f , %.2f) v(%.2f,%.2f)" ,fMoveTargetX, fMoveTargetY, pose_diff.x ,pose_diff.y,vx,vy);
        //     if(fabs(vx) < 0.01 && fabs(vy) < 0.01)
        //     {
        //         VelCmd(0,0,0);
        //         odom_ctrl_msg.data = "pose_diff reset";
        //         odom_ctrl_pub.publish(odom_ctrl_msg);
        //         nTimeDelayCounter = 0;
        //         ROS_WARN("[STEP_DONE]");
        //         nStep = STEP_DONE;
        //     }
        //     mani_ctrl_msg.position[0] = 0.5;
        //     mani_ctrl_pub.publish(mani_ctrl_msg);

        //     result_msg.data = "backward";
        //     result_pub.publish(result_msg);
        // }

        //6、放置任务完毕
        if(nStep == STEP_DONE)
        {
            if(nTimeDelayCounter < 30)
            {
                VelCmd(0,0,0);
                nTimeDelayCounter ++;
                result_msg.data = "done";
                result_pub.publish(result_msg);
            }
            else
            {
                nStep = STEP_WAIT;
            }
        }

        ros::spinOnce();
        r.sleep();
    }

    return 0;
}

<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/navigation.h<|end_filename|>
#ifndef NAVIGATION_H
#define NAVIGATION_H

#include <QWidget>
#include <rviz/visualization_manager.h>
#include <rviz/render_panel.h>
#include <rviz/display.h>
#include <rviz/tool_manager.h>

#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>
#include <rviz/tool.h>
#include <QString>
#include "myviz.h"
#include "fight_with_hair_ui/loadWayPoint.h"
#include <waterplus_map_tools/Waypoint.h>
#include <vector>
#include <fight_with_hair_ui/main_window.hpp>

namespace Ui {
class navigation;
}

class navigation : public QWidget
{
  Q_OBJECT

public:
  explicit navigation(bool isS,QWidget *parent = 0);
  ~navigation();

  void ros_init(int argc,char **argv);

  void setMain(fight_with_hair_ui::MainWindow *);

Q_SIGNALS:
  void navigationmsg_create(WayPoint);
  void navigation_finish();


private slots:
  void on_logout_clicked();

  void on_returnMain_clicked();

  void on_display_button_clicked();

  void on_chooseMap_clicked();

  void getButtonText(bool check);

  void on_chooseDes_clicked();

  void on_chooseWayPoint_clicked();

  void on_cruise_button_clicked();

  void on_save_waypoints_clicked();

private:
  Ui::navigation *ui;
  rviz::VisualizationManager *manager_;
  rviz::RenderPanel *render_panel_;
  rviz::Display *map_;
  rviz::Tool *current_tool_;
  rviz::ToolManager *tool_manager_;
  fight_with_hair_ui::MainWindow *main;

  QString mapfile;
  QString mapname;
  QString xmlfile;

  bool isSim;
protected:
  void closeEvent(QCloseEvent *event);
};

#endif // NAVIGATION_H


<|start_filename|>robot/src/fight_with_hair_ui/src/qnode.cpp<|end_filename|>
/**
 * @file /src/qnode.cpp
 *
 * @brief Ros communication central!
 *
 * @date February 2011
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>
#include <std_msgs/UInt32.h>
#include <sstream>
#include "../include/fight_with_hair_ui/qnode.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace fight_with_hair_ui {

/*****************************************************************************
** Implementation
*****************************************************************************/

QNode::QNode(int argc, char** argv ) :
  init_argc(argc),
  init_argv(argv)
  {}

QNode::~QNode() {
    if(ros::isStarted()) {
      ros::shutdown(); // explicitly needed since we use ros::start();
      ros::waitForShutdown();
    }
  wait();
}

bool QNode::init() {
  ros::init(init_argc,init_argv,"fight_with_hair_ui");
  if ( ! ros::master::check() ) {
    return false;
  }
  ros::start(); // explicitly needed since our nodehandle is going out of scope.
  ros::NodeHandle n;
  // Add your ros communications here.
  chatter_publisher = n.advertise<std_msgs::String>("chatter", 1000);
  vel_publisher = n.advertise<basic::moveMsg>("/fight_with_hair/basic_move/vel",10);
  arm_publisher = n.advertise<arm::armMsg>("/fight_with_hair/arm_move",10);
  objSig_publisher = n.advertise<std_msgs::Bool>("/fight_with_hair/uito/obj_begin",10);
  objDetect_publisher = n.advertise<std_msgs::UInt32>("/fight_with_hair/uito/obj_tar",10);
  objFind_subscriber = n.subscribe<std_msgs::UInt32>("/fight_with_hair/toui/obj_num",10, &QNode::objFind_Listener, this);
  voice_publisher = n.advertise<std_msgs::UInt32>("/fight_with_hair/uito/state_sound", 1);

  voice_subscriber = n.subscribe<std_msgs::String>("/fight_with_hair/toui/sound_ins", 10,&QNode::voice_Listener,this);
  followEnd_publisher = n.advertise<std_msgs::UInt32>("/fight_with_hair/uito/follow_stop", 1);
  followBegin_publisher = n.advertise<std_msgs::UInt32>("/fight_with_hair/uito/follow_begin", 1);
  start();
  return true;
}

bool QNode::init(const std::string &master_url, const std::string &host_url) {
  std::map<std::string,std::string> remappings;
  remappings["__master"] = master_url;
  remappings["__hostname"] = host_url;
  ros::init(remappings,"fight_with_hair_ui");
  if ( ! ros::master::check() ) {
    return false;
  }
  ros::start(); // explicitly needed since our nodehandle is going out of scope.
  ros::NodeHandle n;
  // Add your ros communications here.
  chatter_publisher = n.advertise<std_msgs::String>("chatter", 1000);
  start();
  return true;
}

void QNode::run() {
//  ros::Rate loop_rate(1);
//  int count = 0;
//  while ( ros::ok() ) {

//    std_msgs::String msg;
//    std::stringstream ss;
//    ss << "hello world " << count;
//    msg.data = ss.str();
//    chatter_publisher.publish(msg);
//    log(Info,std::string("I sent: ")+msg.data);
//    ros::spinOnce();
//    loop_rate.sleep();
//    ++count;
//  }
//  std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
//  Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
}


void QNode::log( const LogLevel &level, const std::string &msg) {
  logging_model.insertRows(logging_model.rowCount(),1);
  std::stringstream logging_model_msg;
  switch ( level ) {
    case(Debug) : {
        ROS_DEBUG_STREAM(msg);
        logging_model_msg << "[DEBUG] [" << ros::Time::now() << "]: " << msg;
        break;
    }
    case(Info) : {
        ROS_INFO_STREAM(msg);
        logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
        break;
    }
    case(Warn) : {
        ROS_WARN_STREAM(msg);
        logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
        break;
    }
    case(Error) : {
        ROS_ERROR_STREAM(msg);
        logging_model_msg << "[ERROR] [" << ros::Time::now() << "]: " << msg;
        break;
    }
    case(Fatal) : {
        ROS_FATAL_STREAM(msg);
        logging_model_msg << "[FATAL] [" << ros::Time::now() << "]: " << msg;
        break;
    }
  }
  QVariant new_row(QString(logging_model_msg.str().c_str()));
  logging_model.setData(logging_model.index(logging_model.rowCount()-1),new_row);
  Q_EMIT loggingUpdated(); // used to readjust the scrollbar
}

void QNode::sendMoveMsg(float x, float y, float z){
  basic::moveMsg vel;
  vel.x = x;
  vel.y = y;
  vel.z = z;
  vel_publisher.publish(vel);
}

void QNode::sendArmMsg(float dire_v, float len, float dire_h, float degree){
  arm::armMsg arm_vel;
  arm_vel.direction_v = dire_v;
  arm_vel.length = len;
  arm_vel.direction_h = dire_h;
  arm_vel.degree = degree;
  arm_publisher.publish(arm_vel);
}

void QNode::sendObjSigMsg(bool signal){
  std_msgs::Bool sig;
  sig.data = signal;
  objSig_publisher.publish(sig);
}

void QNode::sendObjDetectMsg(int obj_id){
  std_msgs::UInt32 id;
  id.data = obj_id;
  objDetect_publisher.publish(id);
}

void QNode::objFind_Listener(const std_msgs::UInt32ConstPtr &msg){
  int num = msg->data;
  Q_EMIT QNode::objFind(num);
}

void QNode::sendVoiceSigMsg(){
  std_msgs::UInt32 start;
  start.data = 1;
  voice_publisher.publish(start);
}

void QNode::voice_Listener(const std_msgs::StringConstPtr &msg){
  std::string str = msg->data;
  ROS_INFO("qqqqqq");
  emit asr_order(str);

}

void QNode::sendFollowEndMsg(){
  std_msgs::UInt32 end;
  end.data = 1;
  ROS_INFO("send start");
  followEnd_publisher.publish(end);
}

void QNode::sendFollowBeginMsg(){
  std_msgs::UInt32 begin;
  begin.data = 1;
  followBegin_publisher.publish(begin);
}

}  // namespace fight_with_hair_ui


<|start_filename|>robot/src/drivers/wpb_home/wpbh_local_planner/include/wpbh_local_planner/wl_helper.h<|end_filename|>
#pragma once

void InitHelper();

float GetFixX();
float GetFixY();
float GetFaceX();
float GetFaceY();
void SetRanges(float* inData);
void SetTarget(int inX, int inY);
int GetHelperNum();
bool OutLine();
bool ChkTarget(int inX, int inY);
void ClearObst();
void ClearTarget();
void SetBorder(float inBorder);


<|start_filename|>robot/src/fight_with_hair_ui/src/main.cpp<|end_filename|>
/**
 * @file /src/main.cpp
 *
 * @brief Qt based gui.
 *
 * @date November 2010
 **/
/*****************************************************************************
** Includes
*****************************************************************************/

#include <QtGui>
#include <QApplication>
#include "../include/fight_with_hair_ui/main_window.hpp"
#include "../include/fight_with_hair_ui/loginwindow.h"
#include "../include/fight_with_hair_ui/spinslider.h"
#include "../include/fight_with_hair_ui/build_map_auto.h"

/*****************************************************************************
** Main
*****************************************************************************/

int main(int argc, char **argv) {

  /*********************
    ** Qt
    **********************/
  QApplication app(argc, argv);
  QString str = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch wpb_sim wpb_simple.launch'&";
  //  system(str.toLatin1().data());
  fight_with_hair_ui::MainWindow w(argc,argv);
  LoginWindow *login=new LoginWindow;
  login->setMain(&w);
  login->move(550,200);
  w.move(550,200);
  //  spinslider *n=new spinslider;
  //  n->show();
  //  w.show();
  login->show();

//  build_map_auto *b=new build_map_auto(true);
//  b->move(550,200);
//  b->show();
  app.connect(&app, SIGNAL(lastWindowClosed()), &app, SLOT(quit()));
  int result = app.exec();

  return result;
}


<|start_filename|>robot/src/fight_with_hair_ui/src/mainframe.cpp<|end_filename|>
#include "../include/fight_with_hair_ui/mainframe.h"
#include "ui_mainframe.h"
#include <QMessageBox>

mainframe::mainframe(QWidget *parent) :
  QWidget(parent),
  ui(new Ui::mainframe)
{
  ui->setupUi(this);
  this->setFixedSize(this->geometry().width(),this->geometry().height());

}

mainframe::~mainframe()
{
  delete ui;
}

void mainframe::on_control_with_key_clicked()
{
  userControl *us=new userControl;
  us->move(550,200);
  us->show();
  this->close();
}

void mainframe::on_navigation_clicked()
{
    navigation *na=new navigation(true);
    na->move(550,200);
    na->show();
    this->close();
}

void mainframe::on_build_map_clicked()
{
    build_map *bm=new build_map(true);
    bm->move(550,200);
    bm->show();
    this->close();
}

void mainframe::on_detection_clicked()
{
    detection *de=new detection(1);
    de->move(550,200);
    de->show();
    this->close();
}

void mainframe::on_logout_clicked()
{
    QMessageBox::StandardButton choice=QMessageBox::question(this,"确认","确定要退出吗？",QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes);
    if(choice==QMessageBox::Yes){
        this->close();
        QApplication::quit();
    }
}


<|start_filename|>robot/src/drivers/wpb_home_apps/src/object_find.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* @author <NAME>                                             */
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <vector>
#include <sound_play/SoundRequest.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include "xfyun_waterplus/IATSwitch.h"
#include <waterplus_map_tools/GetWaypointByName.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"

using namespace std;

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
static ros::Publisher spk_pub;
static ros::ServiceClient clientIAT;
static xfyun_waterplus::IATSwitch srvIAT;
static ros::ServiceClient cliGetWPName;
static waterplus_map_tools::GetWaypointByName srvName;
static string strGoto;

//有限状态机
#define STATE_READY         0
#define STATE_WAIT_ENTR     1
#define STATE_GOTO_CMD      2
#define STATE_WAIT_CMD      3
#define STATE_GOTO_REMEMBER 4
#define STATE_WAIT_REMEMBER 5
#define STATE_GOTO_FIND     6
#define STATE_WAIT_FIND     7
#define STATE_LEAVE         8
#define STATE_DONE          9

static int nState = STATE_WAIT_ENTR;  //程序启动时初始状态;

//识别结果
static string remember_placement;     //记忆物品地点
static string remember_object;        //记忆物品
static string find_placement;         //识别物品地点

//识别标记
static bool bRemebered = false;
static bool bFound = false;

//识别关键词
static vector<string> arKWPlacement;
static vector<string> arKWObjece;
static void Init_keywords()
{
    //地点关键词
    arKWPlacement.push_back("kitchen");
    arKWPlacement.push_back("bedroom");
    arKWPlacement.push_back("living room");
    arKWPlacement.push_back("dining room");

    //物品关键词
    arKWObjece.push_back("water");
    arKWObjece.push_back("chips");
    arKWObjece.push_back("milk");

}

static string FindWord(string inSentence, vector<string> & arWord)
{
    string strRes = "";
	int nNum = arWord.size();
	for (int i = 0; i < nNum; i++)
	{
		int tmpIndex = inSentence.find(arWord[i]);
		if (tmpIndex >= 0)
		{
			strRes = arWord[i];
			break;
		}
	}
	return strRes;
}

static void Speak(string inStr)
{
    sound_play::SoundRequest sp;
    sp.sound = sound_play::SoundRequest::SAY;
    sp.command = sound_play::SoundRequest::PLAY_ONCE;
    sp.arg = inStr;
    sp.volume = 1.0f;  //indigo(Ubuntu 14.04)需要注释掉这一句才能编译
    spk_pub.publish(sp);
}

static int nOpenCount = 0;
void EntranceCB(const std_msgs::String::ConstPtr & msg)
{
    //ROS_WARN("[EntranceCB] - %s",msg->data.c_str());
    string strDoor = msg->data;
    if(strDoor == "door open")
    {
        nOpenCount ++;
    }
    else
    {
        nOpenCount = 0;
    }
}

void KeywordCB(const std_msgs::String::ConstPtr & msg)
{
    //ROS_WARN("[KeywordCB] - %s",msg->data.c_str());
    string strListen = msg->data;

    if(nState == STATE_WAIT_CMD)
    {
        bool bPlacement = false;
        bool bObject = false;
        //[1]从听到的句子里找地点
        string placement = FindWord(strListen,arKWPlacement);
        int nLenOfPlacement = strlen(placement.c_str());
        if(nLenOfPlacement > 0)
        {
            printf("句子里包含地点 - %s \n",placement.c_str());
            remember_placement = placement;
            bPlacement = true;
        }

        //[2]从听到的句子里找物品
        string object = FindWord(strListen,arKWObjece);
        int nLenOfObject = strlen(object.c_str());
        if(nLenOfObject > 0)
        {
            printf("句子里包含物品 - %s \n",object.c_str());
            remember_object = object;
            bObject = true;
        }

        if(bPlacement == true && bObject == true)
        {
            // 识别到地点和物品名称后，去往那个地点
            Speak(strListen);
            sleep(3);
            nState = STATE_GOTO_REMEMBER;
        }
    }

    if(nState == STATE_WAIT_REMEMBER)
    {
        //从听到的句子里找地点
        string placement = FindWord(strListen,arKWPlacement);
        int nLenOfPlacement = strlen(placement.c_str());
        if(nLenOfPlacement > 0)
        {
            printf("句子里包含地点 - %s \n",placement.c_str());
            find_placement = placement;
            Speak(strListen);
            sleep(3);
            nState = STATE_GOTO_FIND;
            //识别完毕,暂停语音识别
            srvIAT.request.active = false;
            clientIAT.call(srvIAT);
        }
    }

}

void ProcColorCB(const sensor_msgs::ImageConstPtr& msg)
{
    //ROS_INFO("ProcColorCB");
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    if(nState == STATE_WAIT_REMEMBER)
    {
        if(bRemebered == false)
        {
            // TODO: 对cv_ptr->image里的物品进行记忆处理

            // 保存图片
            imwrite("/home/robot/remember_object.jpg",cv_ptr->image);
            ROS_INFO("[callbackRGB] Save the image of object remembered !");
            Speak("I have remembered it.");
            bRemebered = true;
        }
    }
	
    if(nState == STATE_WAIT_FIND)
    {
        if(bFound == false)
        {
            // TODO: 对cv_ptr->image里的物品进行识别比对，找到记忆物品

            // 保存图片
            imwrite("/home/robot/find_object.jpg",cv_ptr->image);
            ROS_INFO("[callbackRGB] Save the image of object found !");
            string strSpeak = "I have found " + remember_object;
            Speak(strSpeak);
            bFound = true;

            nState = STATE_LEAVE;
        }
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "wpb_home_object_find");
    Init_keywords();

    ros::NodeHandle n;
    ros::Subscriber sub_sr = n.subscribe("/xfyun/iat", 10, KeywordCB);
    ros::Subscriber sub_ent = n.subscribe("/wpb_home/entrance_detect", 10, EntranceCB);
    ros::Subscriber rgb_sub = n.subscribe("/kinect2/qhd/image_color", 1 , ProcColorCB);
    clientIAT = n.serviceClient<xfyun_waterplus::IATSwitch>("xfyun_waterplus/IATSwitch");
    cliGetWPName = n.serviceClient<waterplus_map_tools::GetWaypointByName>("/waterplus/get_waypoint_name");
    spk_pub = n.advertise<sound_play::SoundRequest>("/robotsound", 20);

    ROS_INFO("[main] wpb_home_object_find");
    ros::Rate r(10);
    while(ros::ok())
    {
        if(nState == STATE_WAIT_ENTR)
        {
            //等待开门,一旦检测到开门,便去往发令地点
            if(nOpenCount > 20)
            {
                string strGoto = "cmd";     //cmd是发令地点名称,请在地图里设置这个航点
                srvName.request.name = strGoto;
                if (cliGetWPName.call(srvName))
                {
                    std::string name = srvName.response.name;
                    float x = srvName.response.pose.position.x;
                    float y = srvName.response.pose.position.y;
                    ROS_INFO("Get_wp_name: name = %s (%.2f,%.2f)", strGoto.c_str(),x,y);

                    MoveBaseClient ac("move_base", true);
                    if(!ac.waitForServer(ros::Duration(5.0)))
                    {
                        ROS_INFO("The move_base action server is no running. action abort...");
                    }
                    else
                    {
                        move_base_msgs::MoveBaseGoal goal;
                        goal.target_pose.header.frame_id = "map";
                        goal.target_pose.header.stamp = ros::Time::now();
                        goal.target_pose.pose = srvName.response.pose;
                        ac.sendGoal(goal);
                        ac.waitForResult();
                        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                        {
                            ROS_INFO("Arrived at %s!",strGoto.c_str());
                            Speak("I am ready.");
                            ros::spinOnce();
                            nState = STATE_WAIT_CMD;
                            srvIAT.request.active = true;
                            srvIAT.request.duration = 5;
                            clientIAT.call(srvIAT);
                        }
                        else
                            ROS_INFO("Failed to get to %s ...",strGoto.c_str() );
                    }
                    
                }
                else
                {
                    ROS_ERROR("Failed to call service GetWaypointByName");
                }
            }
        }

        if(nState == STATE_GOTO_REMEMBER)
        {
            strGoto = remember_placement;     //导航目的地航点名称
            srvName.request.name = strGoto;
            if (cliGetWPName.call(srvName))
            {
                std::string name = srvName.response.name;
                float x = srvName.response.pose.position.x;
                float y = srvName.response.pose.position.y;
                ROS_INFO("Get_wp_name: name = %s (%.2f,%.2f)", strGoto.c_str(),x,y);

                MoveBaseClient ac("move_base", true);
                if(!ac.waitForServer(ros::Duration(5.0)))
                {
                    ROS_INFO("The move_base action server is no running. action abort...");
                }
                else
                {
                    move_base_msgs::MoveBaseGoal goal;
                    goal.target_pose.header.frame_id = "map";
                    goal.target_pose.header.stamp = ros::Time::now();
                    goal.target_pose.pose = srvName.response.pose;
                    ac.sendGoal(goal);
                    ac.waitForResult();
                    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                    {
                        ROS_INFO("Arrived at %s!",strGoto.c_str());
                        nState = STATE_WAIT_REMEMBER; 
                        srvIAT.request.active = true;
                        srvIAT.request.duration = 5;
                        clientIAT.call(srvIAT);
                    }
                    else
                        ROS_INFO("Failed to get to %s ...",strGoto.c_str() );
                }
                
            }
            else
            {
                ROS_ERROR("Failed to call service GetWaypointByName");
            }
        }

        if(nState == STATE_GOTO_FIND)
        {
            strGoto = find_placement;     //导航目的地航点名称
            srvName.request.name = strGoto;
            if (cliGetWPName.call(srvName))
            {
                std::string name = srvName.response.name;
                float x = srvName.response.pose.position.x;
                float y = srvName.response.pose.position.y;
                ROS_INFO("Get_wp_name: name = %s (%.2f,%.2f)", strGoto.c_str(),x,y);

                MoveBaseClient ac("move_base", true);
                if(!ac.waitForServer(ros::Duration(5.0)))
                {
                    ROS_INFO("The move_base action server is no running. action abort...");
                }
                else
                {
                    move_base_msgs::MoveBaseGoal goal;
                    goal.target_pose.header.frame_id = "map";
                    goal.target_pose.header.stamp = ros::Time::now();
                    goal.target_pose.pose = srvName.response.pose;
                    ac.sendGoal(goal);
                    ac.waitForResult();
                    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                    {
                        ROS_INFO("Arrived at %s!",strGoto.c_str());
                        nState = STATE_WAIT_FIND; 
                    }
                    else
                        ROS_INFO("Failed to get to %s ...",strGoto.c_str() );
                }
                
            }
            else
            {
                ROS_ERROR("Failed to call service GetWaypointByName");
            }
        }

        if(nState == STATE_LEAVE)
        {
            strGoto = "exit";     //导航目的地航点名称
            srvName.request.name = strGoto;
            if (cliGetWPName.call(srvName))
            {
                std::string name = srvName.response.name;
                float x = srvName.response.pose.position.x;
                float y = srvName.response.pose.position.y;
                ROS_INFO("Get_wp_name: name = %s (%.2f,%.2f)", strGoto.c_str(),x,y);

                MoveBaseClient ac("move_base", true);
                if(!ac.waitForServer(ros::Duration(5.0)))
                {
                    ROS_INFO("The move_base action server is no running. action abort...");
                }
                else
                {
                    move_base_msgs::MoveBaseGoal goal;
                    goal.target_pose.header.frame_id = "map";
                    goal.target_pose.header.stamp = ros::Time::now();
                    goal.target_pose.pose = srvName.response.pose;
                    ac.sendGoal(goal);
                    ac.waitForResult();
                    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                    {
                        ROS_INFO("Arrived at %s!",strGoto.c_str());
                        Speak("I am done.");
                        ros::spinOnce();
                        nState = STATE_DONE; 
                    }
                    else
                        ROS_INFO("Failed to get to %s ...",strGoto.c_str() );
                }
                
            }
            else
            {
                ROS_ERROR("Failed to call service GetWaypointByName");
            }
        }
        
        ros::spinOnce();
        r.sleep();
    }

    return 0;
}

<|start_filename|>robot/src/drivers/wpb_home_apps/include/action_manager.h<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* @author <NAME>                                             */

#ifndef WP_ACTION_MANAGER_H
#define WP_ACTION_MANAGER_H
#include "struct.h"
#include <vector>
#include <sstream>
#include <string.h>
#include <stdlib.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include <pcl/io/pcd_io.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include <sensor_msgs/Image.h>
#include <pcl_ros/transforms.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/segmentation/extract_clusters.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>
#include "xfyun_waterplus/IATSwitch.h"
#include "wpb_home_tutorials/Follow.h"
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <waterplus_map_tools/Waypoint.h>
#include <waterplus_map_tools/GetWaypointByName.h>

using namespace cv;
class CActionManager
{
public:
	CActionManager();
	~CActionManager();

    vector<stAct> arAct;
	int nCurActIndex;
	int nCurActCode;
	std::string strListen;
	int nVideoFrameCount;
	VideoWriter* pVW;
	std::string strImage;
	bool bCaptureImage;

	ros::Subscriber grab_result_sub;
	ros::Subscriber pass_result_sub;
	bool bGrabDone;
	bool bPassDone;

	bool Main();
	void Init();
	void Reset();
	string GetToSpeak();
	void ShowActs();
	void ProcColorCB(const sensor_msgs::ImageConstPtr& msg);
 	void GrabResultCallback(const std_msgs::String::ConstPtr& res);
	void PassResultCallback(const std_msgs::String::ConstPtr& res);
};

#endif // WP_ACTION_MANAGER_H

<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/qnode.hpp<|end_filename|>
/**
 * @file /include/fight_with_hair_ui/qnode.hpp
 *
 * @brief Communications central!
 *
 * @date February 2011
 **/
/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef fight_with_hair_ui_QNODE_HPP_
#define fight_with_hair_ui_QNODE_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

// To workaround boost/qt4 problems that won't be bugfixed. Refer to
//    https://bugreports.qt.io/browse/QTBUG-22829
#ifndef Q_MOC_RUN
#include <ros/ros.h>
#endif

#include <string>
#include <QThread>
#include <QStringListModel>
#include <std_msgs/UInt32.h>
#include <std_msgs/String.h>
#include <basic/moveMsg.h>
#include <arm/armMsg.h>
/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace fight_with_hair_ui {

/*****************************************************************************
** Class
*****************************************************************************/

class QNode : public QThread {
    Q_OBJECT
public:
  QNode(int argc, char** argv );
  virtual ~QNode();
  bool init();
  bool init(const std::string &master_url, const std::string &host_url);
  void run();


  /*********************
  ** Logging
  **********************/
  enum LogLevel {
           Debug,
           Info,
           Warn,
           Error,
           Fatal
   };

  QStringListModel* loggingModel() { return &logging_model; }
  void log( const LogLevel &level, const std::string &msg);
  void sendMoveMsg(float,float,float);
  void sendArmMsg(float,float,float,float);
  void sendObjSigMsg(bool);
  void sendObjDetectMsg(int);
  void sendVoiceSigMsg();
  void objFind_Listener(const std_msgs::UInt32ConstPtr &msg);
  void voice_Listener(const std_msgs::StringConstPtr &msg);
  void sendFollowEndMsg();
  void sendFollowBeginMsg();
Q_SIGNALS:
  void loggingUpdated();
  void rosShutdown();
  void objFind(int);
  void asr_order(std::string);


private:
  int init_argc;
  char** init_argv;
  ros::Publisher chatter_publisher;
    QStringListModel logging_model;
  ros::Publisher vel_publisher;
  ros::Publisher arm_publisher;
  ros::Publisher objSig_publisher;
  ros::Publisher objDetect_publisher;
  ros::Subscriber objFind_subscriber;
  ros::Publisher voice_publisher;
  ros::Subscriber voice_subscriber;
  ros::Publisher followEnd_publisher;
  ros::Publisher followBegin_publisher;
};

}  // namespace fight_with_hair_ui

#endif /* fight_with_hair_ui_QNODE_HPP_ */


<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/myviz.h<|end_filename|>
#ifndef MYVIZ_H
#define MYVIZ_H

#include <QWidget>

namespace rviz
{
class Display;
class RenderPanel;
class VisualizationManager;
}

// BEGIN_TUTORIAL
// Class "MyViz" implements the top level widget for this example.
class MyViz: public QWidget
{
Q_OBJECT
public:
  MyViz( QWidget* parent = 0 );
  virtual ~MyViz();

private Q_SLOTS:
  void setThickness( int thickness_percent );
  void setCellSize( int cell_size_percent );

private:
  rviz::VisualizationManager* manager_;
  rviz::RenderPanel* render_panel_;
  rviz::Display* grid_;
};
// END_TUTORIAL
#endif // MYVIZ_H


<|start_filename|>robot/src/drivers/wpb_home_apps/src/wed_02_shopping.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* @author <NAME>                                             */
#include <ros/ros.h>
#include <std_msgs/String.h>
#include "wpb_home_tutorials/Follow.h"
#include <geometry_msgs/Twist.h>
#include "xfyun_waterplus/IATSwitch.h"
#include <sound_play/SoundRequest.h>
#include "wpb_home_tutorials/Follow.h"
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <waterplus_map_tools/Waypoint.h>
#include <waterplus_map_tools/GetWaypointByName.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>

using namespace std;

#define STATE_READY     0
#define STATE_FOLLOW    1
#define STATE_ASK       2
#define STATE_GOTO      3
#define STATE_GRAB      4
#define STATE_COMEBACK  5
#define STATE_PASS      6

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
static string strGoto;
static sound_play::SoundRequest spk_msg;
static ros::Publisher spk_pub;
static ros::Publisher vel_pub;
static string strToSpeak = "";
static string strKeyWord = "";
static ros::ServiceClient clientIAT;
static xfyun_waterplus::IATSwitch srvIAT;
static ros::ServiceClient cliGetWPName;
static waterplus_map_tools::GetWaypointByName srvName;
static ros::Publisher add_waypoint_pub;
static ros::ServiceClient follow_start;
static ros::ServiceClient follow_stop;
static ros::ServiceClient follow_resume;
static wpb_home_tutorials::Follow srvFlw;
static ros::Publisher behaviors_pub;
static std_msgs::String behavior_msg;

static ros::Subscriber grab_result_sub;
static ros::Subscriber pass_result_sub;
static bool bGrabDone;
static bool bPassDone;

static int nState = STATE_READY;
static int nDelay = 0;

static vector<string> arKeyword;
static vector<string> arObjectWant;
static int nIndexWant = 0;

// 添加航点关键词
void InitKeyword()
{
    arKeyword.push_back("start");   //机器人开始启动的地点,最后要回去
    arKeyword.push_back("water");
    arKeyword.push_back("tea");
    arKeyword.push_back("cola");
}

// 从句子里找arKeyword里存在的关键词
static string FindKeyword(string inSentence)
{
    string res = "";
    int nSize = arKeyword.size();
    for(int i=0;i<nSize;i++)
    {
        int nFindIndex = inSentence.find(arKeyword[i]);
        if(nFindIndex >= 0)
        {
            res = arKeyword[i];
            break;
        }
    }
    return res;
}

// 从句子里找多个arKeyword关键词
static string FindMultiKeyword(string inSentence)
{
    string res = "";
    int nSize = arKeyword.size();
    for(int i=0;i<nSize;i++)
    {
        int nFindIndex = inSentence.find(arKeyword[i]);
        if(nFindIndex >= 0)
        {
            arObjectWant.push_back(arKeyword[i]);
        }
    }
    int nNumObject = arObjectWant.size();
    for(int i=0;i<nNumObject;i++)
    {
        res += arObjectWant[i];
        res += " , ";
    }
    return res;
}

// 将机器人当前位置保存为新航点
void AddNewWaypoint(string inStr)
{
    tf::TransformListener listener;
    tf::StampedTransform transform;
    try
    {
        listener.waitForTransform("/map","/base_footprint",  ros::Time(0), ros::Duration(10.0) );
        listener.lookupTransform("/map","/base_footprint", ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) 
    {
        ROS_ERROR("[lookupTransform] %s",ex.what());
        return;
    }

    float tx = transform.getOrigin().x();
    float ty = transform.getOrigin().y();
    tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(transform.getRotation() , tf::Point(tx, ty, 0.0)), ros::Time::now(), "map");
    geometry_msgs::PoseStamped new_pos;
    tf::poseStampedTFToMsg(p, new_pos);

    waterplus_map_tools::Waypoint new_waypoint;
    new_waypoint.name = inStr;
    new_waypoint.pose = new_pos.pose;
    add_waypoint_pub.publish(new_waypoint);

    ROS_WARN("[New Waypoint] %s ( %.2f , %.2f )" , new_waypoint.name.c_str(), tx, ty);
}

// 语音说话
void Speak(string inStr)
{
    spk_msg.arg = inStr;
    spk_msg.volume = 1.0f;  //indigo(Ubuntu 14.04)需要注释掉这一句才能编译
    spk_pub.publish(spk_msg);
    ros::spinOnce();
}

// 跟随模式开关
static void FollowSwitch(bool inActive, float inDist)
{
    if(inActive == true)
    {
        srvFlw.request.thredhold = inDist;
        if (!follow_start.call(srvFlw))
        {
            ROS_WARN("[CActionManager] - follow start failed...");
        }
    }
    else
    {
        if (!follow_stop.call(srvFlw))
        {
            ROS_WARN("[CActionManager] - failed to stop following...");
        }
    }
}

// 物品抓取模式开关
static void GrabSwitch(bool inActive)
{
    if(inActive == true)
    {
        behavior_msg.data = "grab start";
        behaviors_pub.publish(behavior_msg);
    }
    else
    {
        behavior_msg.data = "grab stop";
        behaviors_pub.publish(behavior_msg);
    }
}

// 物品递给开关
static void PassSwitch(bool inActive)
{
    if(inActive == true)
    {
        behavior_msg.data = "pass start";
        behaviors_pub.publish(behavior_msg);
    }
    else
    {
        behavior_msg.data = "pass stop";
        behaviors_pub.publish(behavior_msg);
    }
}

// 语音识别结果处理函数
void KeywordCB(const std_msgs::String::ConstPtr & msg)
{
    ROS_WARN("------ Keyword = %s ------",msg->data.c_str());
    if(nState == STATE_FOLLOW)
    {
        // 从识别结果句子中查找物品（航点）关键词
        string strKeyword = FindKeyword(msg->data);
        int nLenOfKW = strlen(strKeyword.c_str());
        if(nLenOfKW > 0)
        {
            // 发现物品（航点）关键词
            AddNewWaypoint(strKeyword);
            string strSpeak = strKeyword + " . OK. I have memoried. Next one , please"; 
            Speak(strSpeak);
        }

        // 停止跟随的指令
        int nFindIndex = msg->data.find("top follow");
        if(nFindIndex >= 0)
        {
            FollowSwitch(false, 0);
            AddNewWaypoint("master");
            nState = STATE_ASK;
            nDelay = 0;
            Speak("OK. What do you want me to fetch?");
        }
    }

    if(nState == STATE_ASK)
    {
        // 从识别结果句子中查找多个物品（航点）关键词
        string strKeyword = FindMultiKeyword(msg->data);
        int nLenOfKW = strlen(strKeyword.c_str());
        if(nLenOfKW > 0)
        {
            // 发现物品（航点）关键词
            string strSpeak = "I will repeat the objects. " + strKeyword + "  OK. I will go to get them for you."; 
            Speak(strSpeak);
            sleep(5);   //暂停一会，等机器人把话说完再行动
            nIndexWant = 0;
            nState = STATE_GOTO;
        }
    }
}

// 物品抓取状态
void GrabResultCallback(const std_msgs::String::ConstPtr& res)
{
    int nFindIndex = 0;
    nFindIndex = res->data.find("done");
    if( nFindIndex >= 0 )
    {
        bGrabDone = true;
    }
}

// 物品递给状态
void PassResultCallback(const std_msgs::String::ConstPtr& res)
{
    int nFindIndex = 0;
    nFindIndex = res->data.find("done");
    if( nFindIndex >= 0 )
    {
        bPassDone = true;
    }
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "wpb_home_shopping");

    ros::NodeHandle n;
    ros::Subscriber sub_sr = n.subscribe("/xfyun/iat", 10, KeywordCB);
    follow_start = n.serviceClient<wpb_home_tutorials::Follow>("wpb_home_follow/start");
    follow_stop = n.serviceClient<wpb_home_tutorials::Follow>("wpb_home_follow/stop");
    follow_resume = n.serviceClient<wpb_home_tutorials::Follow>("wpb_home_follow/resume");
    cliGetWPName = n.serviceClient<waterplus_map_tools::GetWaypointByName>("/waterplus/get_waypoint_name");
    add_waypoint_pub = n.advertise<waterplus_map_tools::Waypoint>( "/waterplus/add_waypoint", 1);
    spk_pub = n.advertise<sound_play::SoundRequest>("/robotsound", 20);
    spk_msg.sound = sound_play::SoundRequest::SAY;
    spk_msg.command = sound_play::SoundRequest::PLAY_ONCE;
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    clientIAT = n.serviceClient<xfyun_waterplus::IATSwitch>("xfyun_waterplus/IATSwitch");
    behaviors_pub = n.advertise<std_msgs::String>("/wpb_home/behaviors", 30);
    grab_result_sub = n.subscribe<std_msgs::String>("/wpb_home/grab_result",30,&GrabResultCallback);
    pass_result_sub = n.subscribe<std_msgs::String>("/wpb_home/pass_result",30,&PassResultCallback);

    InitKeyword();

    ROS_WARN("[main] wed_02_shopping");
    ros::Rate r(30);
	
    while(ros::ok())
    {
        // 1、刚启动，准备
        if(nState == STATE_READY)
        {
            // 启动后延迟一段时间然后开始跟随
            nDelay ++;
            // ROS_WARN("[STATE_READY] - nDelay = %d", nDelay);
            if(nDelay > 100)
            {
                AddNewWaypoint("start");
                nDelay = 0;
                nState = STATE_FOLLOW;
            }
            string hello_string = "欢迎使用爱死机机机机机死机";
	        Speak(hello_string);
        }

        // 2、跟随阶段
        if(nState == STATE_FOLLOW)
        {
            if(nDelay == 0)
            {
               FollowSwitch(true, 0.7);
            }
            nDelay ++;
        }

        // 3、询问要去哪个航点
        if(nState == STATE_ASK)
        {
            
        }

        // 4、导航去指定航点
        if(nState == STATE_GOTO)
        {
            int nNumWant = arObjectWant.size();
            if(nIndexWant < nNumWant)
            {
                strGoto = arObjectWant[nIndexWant];
            }
            srvName.request.name = strGoto;
            if (cliGetWPName.call(srvName))
            {
                std::string name = srvName.response.name;
                float x = srvName.response.pose.position.x;
                float y = srvName.response.pose.position.y;
                ROS_INFO("[STATE_GOTO] Get_wp_name = %s (%.2f,%.2f)", strGoto.c_str(),x,y);

                MoveBaseClient ac("move_base", true);
                if(!ac.waitForServer(ros::Duration(5.0)))
                {
                    ROS_INFO("The move_base action server is no running. action abort...");
                }
                else
                {
                    move_base_msgs::MoveBaseGoal goal;
                    goal.target_pose.header.frame_id = "map";
                    goal.target_pose.header.stamp = ros::Time::now();
                    goal.target_pose.pose = srvName.response.pose;
                    ac.sendGoal(goal);
                    ac.waitForResult();
                    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                    {
                        ROS_INFO("Arrived at %s!",strGoto.c_str());
                        Speak("这就帮您取回来！");
                        nState = STATE_GRAB;
                        nDelay = 0;
                    }
                    else
                    {
                        ROS_INFO("Failed to get to %s ...",strGoto.c_str() );
                        Speak("抱歉，我去不了这个航点");
                        nState = STATE_ASK;
                    }
                }
                
            }
            else
            {
                ROS_ERROR("Failed to call service GetWaypointByName");
                Speak("There is no this waypoint.");
                nState = STATE_ASK;
            }
        }

        // 5、抓取物品
        if(nState == STATE_GRAB)
        {
            if(nDelay == 0)
            {
                bGrabDone = false;
                GrabSwitch(true);
            }
            nDelay ++;
            if(bGrabDone == true)
            {
                GrabSwitch(false);
                Speak("我拿到了！这就回来！");
                nState = STATE_COMEBACK;
            }
        }

        // 6、抓完物品回来
        if(nState == STATE_COMEBACK)
        {
            srvName.request.name = "master";
            if (cliGetWPName.call(srvName))
            {
                std::string name = srvName.response.name;
                float x = srvName.response.pose.position.x;
                float y = srvName.response.pose.position.y;
                ROS_INFO("[STATE_COMEBACK] Get_wp_name = %s (%.2f,%.2f)", strGoto.c_str(),x,y);

                MoveBaseClient ac("move_base", true);
                if(!ac.waitForServer(ros::Duration(5.0)))
                {
                    ROS_INFO("The move_base action server is no running. action abort...");
                }
                else
                {
                    move_base_msgs::MoveBaseGoal goal;
                    goal.target_pose.header.frame_id = "map";
                    goal.target_pose.header.stamp = ros::Time::now();
                    goal.target_pose.pose = srvName.response.pose;
                    ac.sendGoal(goal);
                    ac.waitForResult();
                    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                    {
                        ROS_INFO("Arrived at %s!",strGoto.c_str());
                        Speak("嘿主人！这就是你想要的东西");
                        nState = STATE_PASS;
                        nDelay = 0;
                    }
                    else
                    {
                        ROS_INFO("Failed to get to %s ...",strGoto.c_str() );
                        Speak("抱歉，没法回到主人身边");
                        nState = STATE_ASK;
                    }
                }
                
            }
            else
            {
                ROS_ERROR("Failed to call service GetWaypointByName");
                Speak("没有叫 master 的航点");
                nState = STATE_ASK;
            }
        }

        // 7、将物品给主人
        if(nState == STATE_PASS)
        {
            if(nDelay == 0)
            {
                bPassDone = false;
                PassSwitch(true);
            }
            nDelay ++;
            if(bPassDone == true)
            {
                PassSwitch(false);
                nIndexWant ++;
                int nNumWant = arObjectWant.size();
                if(nIndexWant < nNumWant)
                {
                    string strSpeak = "OK. I am going to get the next , "+arObjectWant[nIndexWant];
                    Speak(strSpeak);
                    sleep(3);   //暂停一会，让机器人把话说完
                    nState = STATE_GOTO;
                }
                else
                {
                    Speak("全办妥了！您还想要其他的东西吗？");
                    nState = STATE_ASK;
                }
            }
        }
        
        ros::spinOnce();
        r.sleep();
    }

    return 0;
}


<|start_filename|>robot/src/fight_with_hair_ui/src/main_window.cpp<|end_filename|>
/**
 * @file /src/main_window.cpp
 *
 * @brief Implementation for the qt gui.
 *
 * @date February 2011
 **/
/*****************************************************************************
** Includes
*****************************************************************************/

#include <QtGui>
#include <QMessageBox>
#include <iostream>
#include "../include/fight_with_hair_ui/main_window.hpp"
#include "../include/fight_with_hair_ui/mainframe.h"
#include "../include/fight_with_hair_ui/usercontrol.h"
#include "../include/fight_with_hair_ui/build_map.h"
#include "../include/fight_with_hair_ui/detection.h"
#include "../include/fight_with_hair_ui/spinslider.h"
#include "../include/fight_with_hair_ui/navigation.h"
#include "../include/fight_with_hair_ui/follow.h"
#include "../include/fight_with_hair_ui/build_map_auto.h"
#include <QPixmap>
#include <QLabel>
#include <QCloseEvent>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace fight_with_hair_ui {

using namespace Qt;

/*****************************************************************************
** Implementation [MainWindow]
*****************************************************************************/

MainWindow::MainWindow(int argc, char** argv, QWidget *parent)
  : QMainWindow(parent)
  , qnode(argc,argv)
{
  ui.setupUi(this); // Calling this incidentally connects all ui's triggers to on_...() callbacks in this class.
  this->setFixedSize(this->geometry().width(),this->geometry().height());
  ReadSettings();
  setWindowIcon(QIcon(":/images/icon.png"));
  QPixmap *ro=new QPixmap(":/images/robot.png");
  ro->scaled(ui.pic->size(),Qt::KeepAspectRatio);
  ui.pic->setScaledContents(true);
  ui.pic->setPixmap(*ro);
  qnode.init();

}

MainWindow::~MainWindow() {}

/*****************************************************************************
** Implementation [Slots]
*****************************************************************************/

/*****************************************************************************
** Implementation [Menu]
*****************************************************************************/
void MainWindow::ReadSettings() {

}

void MainWindow::WriteSettings() {

}

void MainWindow::closeEvent(QCloseEvent *event)
{
  WriteSettings();
  QString string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /joint_state_publisher;rosnode kill /robot_state_publisher;'&";
  system(string.toLatin1().data());
  QMainWindow::closeEvent(event);
}

}  // namespace fight_with_hair_ui


void fight_with_hair_ui::MainWindow::on_control_with_key_clicked()
{
  //  qnode.sendMoveMsg(1,0,0);
  userControl *us=new userControl;
  us->move(550,200);
  us->setMain(this);
  this->hide();
  us->show();
  //  connect(us,SIGNAL(movemsg(float,float,float)),&qnode,SLOT(sendMoveMsg(float,float,float)));
  connect(us,&userControl::movemsg,&qnode,&QNode::sendMoveMsg);
  connect(us,&userControl::armmsg,&qnode,&QNode::sendArmMsg);
  connect(us,&userControl::userControl_finish,&qnode,&QNode::sendVoiceSigMsg);
}

void fight_with_hair_ui::MainWindow::on_navigation_clicked()
{
  navigation *na=new navigation(isSim);
  na->move(550,200);
  na->setMain(this);
  this->hide();
  na->show();
  connect(na,&navigation::navigation_finish,&qnode,&QNode::sendVoiceSigMsg);
}

void fight_with_hair_ui::MainWindow::on_build_map_clicked()
{
  build_map *bm=new build_map(isSim);
  bm->move(550,200);
  bm->setMain(this);
  this->hide();
  bm->show();
  connect(bm,&build_map::movemsg,&qnode,&QNode::sendMoveMsg);
  connect(bm,&build_map::build_map_finish,&qnode,&QNode::sendVoiceSigMsg);
}


void fight_with_hair_ui::MainWindow::on_detection_clicked()
{
  detection *de=new detection(isSim);
  de->move(550,200);
  de->setMain(this);
  this->hide();
  de->show();
  connect(de,&detection::sendObjectNum,&qnode,&QNode::sendObjDetectMsg);
  connect(&qnode,&QNode::objFind,de,&detection::setObjectSize);
  connect(de,&detection::sendDetectMsg,&qnode,&QNode::sendObjSigMsg);
  connect(de,&detection::detection_finish,&qnode,&QNode::sendVoiceSigMsg);
}

void fight_with_hair_ui::MainWindow::on_logout_clicked()
{
  QMessageBox::StandardButton choice=QMessageBox::question(this,"确认","确定要退出吗？",QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes);
  if(choice==QMessageBox::Yes){
    this->close();
    QString string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /joint_state_publisher;rosnode kill /robot_state_publisher;'&";
    system(string.toLatin1().data());
    QApplication::quit();
  }
}

void fight_with_hair_ui::MainWindow::on_follow_clicked()
{
  Follow *fo=new Follow(isSim);
  fo->move(550,200);
  fo->setMain(this);
  this->hide();
  fo->show();
  connect(fo,&Follow::send_follow_begin_msg,&qnode,&QNode::sendFollowBeginMsg);
  connect(fo,&Follow::send_follow_end_msg,&qnode,&QNode::sendFollowEndMsg);
  connect(fo,&Follow::follow_finish,&qnode,&QNode::sendVoiceSigMsg);
}

void fight_with_hair_ui::MainWindow::on_ASR_clicked()
{
  if (isSim){
    QString ASRstrTest = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch fwh_sound fwh_sound_t.launch'&";
    system(ASRstrTest.toLatin1().data());
  }else{
    QString ASRstr = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch fwh_sound fwh_sound_l.launch'&";
    system(ASRstr.toLatin1().data());
  }

  connect(&qnode,&QNode::asr_order,this,&MainWindow::receive_asr_orders);
  connect(this,&MainWindow::send_follow_end_msg,&qnode,&QNode::sendFollowEndMsg);
}

void fight_with_hair_ui::MainWindow::receive_asr_orders(std::string str){
  if(str=="obj"){
    on_detection_clicked();
    return;
  }
  if(str=="navi"){
    on_navigation_clicked();
    return;
  }
  if(str=="mani"){
    on_control_with_key_clicked();
    return;
  }
  if (str=="follow"){
    on_follow_clicked();
    return;
  }
  if (str=="build"){
    on_build_map_clicked();
    return;
  }
  if (str=="follow_end"){
    emit send_follow_end_msg();
    return;
  }
}

void fight_with_hair_ui::MainWindow::chooseEnvironment(){
  QMessageBox::StandardButton choice=QMessageBox::question(this,"Question","是否在机器人上运行？",QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes);
  if(choice==QMessageBox::Yes){
    isSim=false;
  }else{
    isSim=true;
  }
}

void fight_with_hair_ui::MainWindow::on_aaaaa_clicked()
{
  QString yanshi = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch wpb_home_apps innovation.launch'&";
  system(yanshi.toLatin1().data());
}

void fight_with_hair_ui::MainWindow::on_autoMap_clicked()
{
  build_map_auto *bma=new build_map_auto(isSim);
  bma->move(550,200);
  bma->setMain(this);
  this->hide();
  bma->show();
}


<|start_filename|>robot/src/drivers/wpb_home/wpb_home_bringup/src/wpb_home_core.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*!******************************************************************
 @author     ZhangWanjie
 ********************************************************************/

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32MultiArray.h>
#include "driver/WPB_Home_driver.h"
#include <math.h>

static CWPB_Home_driver m_wpb_home;
static int nLastMotorPos[3];
static bool ad_publish_enable = true;
static std_msgs::Int32MultiArray ad_msg;
static bool input_publish_enable = true;
static std_msgs::Int32MultiArray input_msg;
static int arOutput[8];

void CmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
    //ROS_INFO("[wpb_home] liner(%.2f %.2f) angular(%.2f)", msg->linear.x,msg->linear.y,msg->angular.z);
    m_wpb_home.Velocity(msg->linear.x,msg->linear.y,msg->angular.z);
}

static float kForearm = 1.57/11200;
static float fLiftValue = 0;
static float fLiftVelocity = 0;
static float fGripperValue = 0;
static float fGripperVelocity = 0;
void ManiCtrlCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
    int nNumJoint = msg->position.size();
    // for(int i=0;i<nNumJoint;i++)
    // {
    //     ROS_INFO("[wpb_home] %d - %s = %.2f  vel= %.2f", i, msg->name[i].c_str(),msg->position[i],msg->velocity[i]);
    // }
    //高度升降
    fLiftValue = msg->position[0];
    fLiftVelocity = msg->velocity[0];
    //手爪
    fGripperValue = msg->position[1];
    fGripperVelocity = msg->velocity[1];

    m_wpb_home.ManiCmd(fLiftValue, fLiftVelocity, fGripperValue, fGripperVelocity);
}

static geometry_msgs::Pose2D pose_diff_msg;
void CtrlCallback(const std_msgs::String::ConstPtr &msg)
{
    int nFindIndex = 0;
    nFindIndex = msg->data.find("pose_diff reset");
    if( nFindIndex >= 0 )
    {
        pose_diff_msg.x = 0;
        pose_diff_msg.y = 0;
        pose_diff_msg.theta = 0;
        //ROS_WARN("[pose_diff reset]");
    }
    nFindIndex = msg->data.find("motor encoder");
    if( nFindIndex >= 0 )
    {
        printf("\n[电机码盘位置] 电机1= %d    电机2= %d    电机3= %d\n", m_wpb_home.arMotorPos[0], m_wpb_home.arMotorPos[1], m_wpb_home.arMotorPos[2]);
    }
}

void OutputCallback(const std_msgs::Int32MultiArray::ConstPtr &msg)
{
    int nNumOutput =  msg->data.size();
    if(nNumOutput > 8)
        nNumOutput = 8;

    for(int i=0;i<nNumOutput;i++)
    {
        arOutput[i] = msg->data[i];
    }
    m_wpb_home.Output(arOutput);
}

static float fKVx = 1.0f/sqrt(3.0f);
static float fKVy = 2.0f/3.0f;
static float fKVz = 1.0f/3.0f;
static float fSumX =0;
static float fSumY =0;
static float fSumZ =0;
static float fOdomX =0;
static float fOdomY =0;
static float fOdomZ =0;
static geometry_msgs::Pose2D lastPose;
static geometry_msgs::Twist lastVel;
int main(int argc, char** argv)
{
    ros::init(argc,argv,"wpb_home_core");
    ros::NodeHandle n;
    ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel",10,&CmdVelCallback);
    ros::Subscriber mani_ctrl_sub = n.subscribe("/wpb_home/mani_ctrl",30,&ManiCtrlCallback);
    ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu >("imu/data_raw", 100);
    ros::Publisher ad_pub = n.advertise<std_msgs::Int32MultiArray>("/wpb_home/ad", 10);
    ros::Publisher input_pub = n.advertise<std_msgs::Int32MultiArray>("/wpb_home/input", 10);

    for(int i=0;i<8;i++)
        arOutput[i] = 0;
    ros::Subscriber output_sub = n.subscribe("/wpb_home/output",10,&OutputCallback);

    ros::NodeHandle n_param("~");
    std::string strSerialPort;
    n_param.param<std::string>("serial_port", strSerialPort, "/dev/ttyUSB0");
    m_wpb_home.Open(strSerialPort.c_str(),115200);

    bool bImuOdom;
    n_param.param<bool>("imu_odom", bImuOdom, false);
    
    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();
    ros::Rate r(100.0);

    ros::Publisher joint_state_pub = n.advertise<sensor_msgs::JointState>("/joint_states",100);
    sensor_msgs::JointState joint_msg;
    std::vector<std::string> joint_name(6);
    std::vector<double> joint_pos(6);

    joint_name[0] = "mani_base";
    joint_name[1] = "elbow_forearm";
    joint_name[2] = "forearm_left_finger";
    joint_name[3] = "forearm_right_finger";
    joint_name[4] = "kinect_height";
    joint_name[5] = "kinect_pitch";
    joint_pos[0] = 0.0f;
    joint_pos[1] = 0.0f;
    joint_pos[2] = 0.0f;
    joint_pos[3] = 0.0f;
    joint_pos[4] = 0.0f;
    joint_pos[5] = 0.0f;
    n_param.getParam("zeros/kinect_height", joint_pos[4]);
    n_param.getParam("zeros/kinect_pitch", joint_pos[5]);

    ros::Publisher odom_pub;
    geometry_msgs::TransformStamped odom_trans;
    tf::TransformBroadcaster broadcaster;
    nav_msgs::Odometry odom;
    geometry_msgs::Quaternion odom_quat;

    odom_pub = n.advertise<nav_msgs::Odometry>("odom",2);
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_footprint";
    odom.header.frame_id = "odom";
    odom.child_frame_id = "base_footprint";
    odom_trans.transform.translation.x = 0;
    odom_trans.transform.translation.y = 0;
    odom_trans.transform.translation.z = 0;
    odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(0);
    odom.pose.pose.position.x = 0;
    odom.pose.pose.position.y = 0;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;
    odom.twist.twist.linear.x = 0;
    odom.twist.twist.linear.y = 0;
    odom.twist.twist.linear.z = 0;
    odom.twist.twist.angular.x = 0;
    odom.twist.twist.angular.y = 0;
    odom.twist.twist.angular.z = 0;

    ros::Subscriber ctrl_sub = n.subscribe("/wpb_home/ctrl",10,&CtrlCallback);
    ros::Publisher pose_diff_pub = n.advertise<geometry_msgs::Pose2D>("/wpb_home/pose_diff",1);
    pose_diff_msg.x = 0;
    pose_diff_msg.y = 0;
    pose_diff_msg.theta = 0;

    lastPose.x = lastPose.y = lastPose.theta = 0;
    lastVel.linear.x = lastVel.linear.y = lastVel.linear.z = lastVel.angular.x = lastVel.angular.y = lastVel.angular.z = 0;
    nLastMotorPos[0] = nLastMotorPos[1] = nLastMotorPos[2] = 0;
    while(n.ok())
    {
        m_wpb_home.ReadNewData();
        m_wpb_home.nParseCount ++;
        //ROS_INFO("[m_wpb_home.nParseCount]= %d",m_wpb_home.nParseCount);
        if(m_wpb_home.nParseCount > 100)
        {
            m_wpb_home.arMotorPos[0] =0; nLastMotorPos[0] = 0;
            m_wpb_home.arMotorPos[1] =0; nLastMotorPos[1] = 0;
            m_wpb_home.arMotorPos[2] =0; nLastMotorPos[2] = 0;
            m_wpb_home.nParseCount = 0;
            //ROS_INFO("empty");
        }
        
        last_time = current_time;
        current_time = ros::Time::now();

        if(bImuOdom == false)
        {
            double fVx,fVy,fVz;
            double fPosDiff[3];
            if(nLastMotorPos[0] != m_wpb_home.arMotorPos[0] || nLastMotorPos[1] != m_wpb_home.arMotorPos[1] || nLastMotorPos[2] != m_wpb_home.arMotorPos[2])
            {
                fPosDiff[0] = (double)(m_wpb_home.arMotorPos[0] - nLastMotorPos[0]); 
                fPosDiff[1] = (double)(m_wpb_home.arMotorPos[1] - nLastMotorPos[1]);
                fPosDiff[2] = (double)(m_wpb_home.arMotorPos[2] - nLastMotorPos[2]);
                
                fVx = (fPosDiff[1] - fPosDiff[0]) * fKVx;
                fVy = (fPosDiff[0] + fPosDiff[1]) - (fPosDiff[0] + fPosDiff[1] + fPosDiff[2])*fKVy;
                fVz = (fPosDiff[0] + fPosDiff[1] + fPosDiff[2])*fKVz;
                double fTimeDur = current_time.toSec() - last_time.toSec();
                fVx = fVx/(fTimeDur*9100);
                fVy = fVy/(fTimeDur*9100);
                fVz = fVz/(fTimeDur*1840);
                
                double dx = (lastVel.linear.x*cos(lastPose.theta) - lastVel.linear.y*sin(lastPose.theta))*fTimeDur;
                double dy = (lastVel.linear.x*sin(lastPose.theta) + lastVel.linear.y*cos(lastPose.theta))*fTimeDur;

                lastPose.x += dx;
                lastPose.y += dy;
                lastPose.theta += (fVz*fTimeDur);

                double pd_dx = (lastVel.linear.x*cos(pose_diff_msg.theta) - lastVel.linear.y*sin(pose_diff_msg.theta))*fTimeDur;
                double pd_dy = (lastVel.linear.x*sin(pose_diff_msg.theta) + lastVel.linear.y*cos(pose_diff_msg.theta))*fTimeDur;
                pose_diff_msg.x += pd_dx;
                pose_diff_msg.y += pd_dy;
                pose_diff_msg.theta += (fVz*fTimeDur);

                odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,lastPose.theta);
                //updata transform
                odom_trans.header.stamp = current_time;
                odom_trans.transform.translation.x = lastPose.x;
                odom_trans.transform.translation.y = lastPose.y;
                odom_trans.transform.translation.z = 0;
                odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(lastPose.theta);

                //filling the odometry
                odom.header.stamp = current_time;
                //position
                odom.pose.pose.position.x = lastPose.x;
                odom.pose.pose.position.y = lastPose.y;
                odom.pose.pose.position.z = 0.0;
                odom.pose.pose.orientation = odom_quat;
                //velocity
                odom.twist.twist.linear.x = fVx;
                odom.twist.twist.linear.y = fVy;
                odom.twist.twist.linear.z = 0;
                odom.twist.twist.angular.x = 0;
                odom.twist.twist.angular.y = 0;
                odom.twist.twist.angular.z = fVz;

                //plublishing the odometry and new tf
                broadcaster.sendTransform(odom_trans);
                odom_pub.publish(odom);

                lastVel.linear.x = fVx;
                lastVel.linear.y = fVy;
                lastVel.angular.z = fVz;

                nLastMotorPos[0] = m_wpb_home.arMotorPos[0];
                nLastMotorPos[1] = m_wpb_home.arMotorPos[1];
                nLastMotorPos[2] = m_wpb_home.arMotorPos[2];
            }
            else
            {
                odom_trans.header.stamp = ros::Time::now();
                //plublishing the odometry and new tf
                broadcaster.sendTransform(odom_trans);
                odom.header.stamp = ros::Time::now();
                odom_pub.publish(odom);
                //ROS_INFO("[odom] zero");
            }

            pose_diff_pub.publish(pose_diff_msg);
            //ROS_INFO("[pose_diff_msg] x= %.2f  y=%.2f  th= %.2f", pose_diff_msg.x,pose_diff_msg.y,pose_diff_msg.theta);
        }
        else
        {
            //imu
            sensor_msgs::Imu imu_msg = sensor_msgs::Imu();	
            imu_msg.header.stamp = ros::Time::now();
            imu_msg.header.frame_id = "imu";
            imu_msg.orientation.x = m_wpb_home.fQuatX;
            imu_msg.orientation.y = m_wpb_home.fQuatY;
            imu_msg.orientation.z = m_wpb_home.fQuatZ;
            imu_msg.orientation.w = m_wpb_home.fQuatW;

            imu_msg.angular_velocity.x = m_wpb_home.fGyroX;
            imu_msg.angular_velocity.y = m_wpb_home.fGyroY;
            imu_msg.angular_velocity.z = m_wpb_home.fGyroZ;

            imu_msg.linear_acceleration.x = m_wpb_home.fAccX;
            imu_msg.linear_acceleration.y = m_wpb_home.fAccY;
            imu_msg.linear_acceleration.z = m_wpb_home.fAccZ;

            imu_pub.publish(imu_msg);
        }

        // mani tf
        joint_msg.header.stamp = ros::Time::now();
        joint_msg.header.seq ++;
        joint_pos[0] = m_wpb_home.arMotorPos[4] * 0.00001304;
        if(m_wpb_home.arMotorPos[4] < 11200)
        {
            joint_pos[1] = m_wpb_home.arMotorPos[4]*kForearm;
        }
        else
        {
            joint_pos[1] = 1.57;
        }
        joint_pos[2] = (47998 - m_wpb_home.arMotorPos[5]) * 0.00001635;
        joint_pos[3] = joint_pos[2];
        joint_msg.name = joint_name;
        joint_msg.position = joint_pos;
        joint_state_pub.publish(joint_msg);

        // ad
        if(ad_publish_enable == true)
        {
            ad_msg.data.clear();
            for(int i=0;i<15;i++)
            {
                ad_msg.data.push_back(m_wpb_home.arValAD[i]); 
            }
            ad_pub.publish(ad_msg);
        }

        //input
        if(input_publish_enable == true)
        {
            input_msg.data.clear();
            for(int i=0;i<4;i++)
            {
                input_msg.data.push_back(m_wpb_home.arValIOInput[i]); 
            }
            input_pub.publish(input_msg);
        }

        ros::spinOnce();
        r.sleep();
    }
}

<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/follow.h<|end_filename|>
#ifndef FOLLOW_H
#define FOLLOW_H

#include <QWidget>
#include <fight_with_hair_ui/main_window.hpp>

namespace Ui {
class Follow;
}

class Follow : public QWidget
{
  Q_OBJECT

public:
  explicit Follow(bool isSim,QWidget *parent = 0);
  ~Follow();

  void setMain(fight_with_hair_ui::MainWindow*);

Q_SIGNALS:
  void send_follow_begin_msg();
  void send_follow_end_msg();
  void follow_finish();


private slots:
  void on_returnMain_clicked();

  void on_logout_clicked();

  void on_follow_begin_clicked();

  void on_follow_end_clicked();

private:
  Ui::Follow *ui;
  fight_with_hair_ui::MainWindow *main;

protected:
  void closeEvent(QCloseEvent *event);
};

#endif // FOLLOW_H


<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/mainframe.h<|end_filename|>
#ifndef MAINFRAME_H
#define MAINFRAME_H

#include <QWidget>
#include "usercontrol.h"
#include "build_map.h"
#include "detection.h"
#include "spinslider.h"
#include "navigation.h"

namespace Ui {
class mainframe;
}

class mainframe : public QWidget
{
  Q_OBJECT

public:
  explicit mainframe(QWidget *parent = 0);
  ~mainframe();

private slots:
  void on_control_with_key_clicked();

  void on_navigation_clicked();

  void on_build_map_clicked();

  void on_detection_clicked();

  void on_logout_clicked();

private:
  Ui::mainframe *ui;
};

#endif // MAINFRAME_H


<|start_filename|>robot/src/fight_with_hair_ui/src/loginwindow.cpp<|end_filename|>
#include "../include/fight_with_hair_ui/loginwindow.h"
#include "ui_loginwindow.h"
#include <QString>
#include <QMessageBox>
#include <QCloseEvent>
#include <QLineEdit>

LoginWindow::LoginWindow(QWidget *parent) :
  QWidget(parent),
  ui(new Ui::LoginWindow)
{
  ui->setupUi(this);
  this->setFixedSize(this->geometry().width(),this->geometry().height());
  ui->input_password->setEchoMode(QLineEdit::Password);
}

LoginWindow::~LoginWindow()
{
  delete ui;
}

void LoginWindow::on_loginButton_clicked()
{
  QString user=ui->input_name->text();
  QString password=ui->input_password->text();
  if(user.isEmpty()||password.isEmpty()){
    QMessageBox::information(NULL, tr("提示"),tr("用户名或密码不能为空！"));
    return;
  }
  if(user=="fwh"&&password=="1"){
    // QMessageBox::information(NULL, "提示", "登陆成功",QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
    QMessageBox::information(this, tr("提示"),tr("登陆成功！"));
    main->show();
    this->close();
    main->chooseEnvironment();
  }else{
    QMessageBox::information(this, tr("警告"),tr("用户名或密码错误！"));
    return;
  }
}

void LoginWindow::on_input_password_returnPressed()
{
  on_loginButton_clicked();
}

void LoginWindow::on_resetButton_clicked()
{
  ui->input_name->setText("");
  ui->input_password->setText("");
}

void LoginWindow::setMain(fight_with_hair_ui::MainWindow *m){
  main=m;
}

void LoginWindow::closeEvent(QCloseEvent *event){
}


<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/loginwindow.h<|end_filename|>
#ifndef LOGINWINDOW_H
#define LOGINWINDOW_H

#include <QWidget>
#include "mainframe.h"
#include <fight_with_hair_ui/main_window.hpp>

namespace Ui {
class LoginWindow;
}

class LoginWindow : public QWidget
{
  Q_OBJECT

public:
  explicit LoginWindow(QWidget *parent = 0);
  ~LoginWindow();

  void setMain(fight_with_hair_ui::MainWindow *);

private slots:
  void on_loginButton_clicked();

  void on_input_password_returnPressed();

  void on_resetButton_clicked();

private:
  Ui::LoginWindow *ui;
  fight_with_hair_ui::MainWindow *main;

protected:
  void closeEvent(QCloseEvent *event);
};

#endif // LOGINWINDOW_H


<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/cctrldashboard.h<|end_filename|>
#ifndef CCTRLDASHBOARD_H
#define CCTRLDASHBOARD_H

#include <QWidget>

class CCtrlDashBoard : public QWidget
{
    Q_OBJECT
public:
    enum StyleType {
        ST_DEFAULT=0,
        ST_ARCBAR
    };
    explicit CCtrlDashBoard(QWidget *parent = 0, StyleType type=ST_DEFAULT);

    void setUnit(int unit){
        m_Unit = unit;
        update();
    }

    void setValue(qreal value){
        m_DashValue = value + 50;
        update();
    }
    void setBackGroundColor(QColor color){
        m_BgColor=color;
        update();
    }
    void setFrontColor(QColor color){
        m_FrontColor=color;
        update();
    }
    void setBorderColor(QColor color){
        m_BorderColor=color;
        update();
    }
    void setUnitString(QString str){
        m_StrUnit=str;
        update();
    }

    void drawBackGround(QPainter *painter, qreal hlafWidth);
    void drawScaleDials(QPainter *painter, qreal hlafWidth);
    void drawIndicator(QPainter *painter, qreal hlafWidth);
    void drawIndicatorBar(QPainter *painter, qreal hlafWidth);
signals:

public slots:
protected:
    virtual void paintEvent(QPaintEvent * event);

private:
    int m_StartAngle;
    int m_EndAngle;
    int m_StyleType;
    int m_Unit;

    qreal m_LineLength;
    qreal m_DashValue;
    qreal m_MaxValue;
    qreal m_MinValue;
    qreal m_DashNum;

    QColor m_BgColor;
    QColor m_FrontColor;
    QColor m_BorderColor;
    QString m_StrUnit;

    qreal m_MaxBorderRadius;
    qreal m_MinBorderRadius;
    qreal m_DialsRadius;
};

#endif // CCTRLDASHBOARD_H


<|start_filename|>robot/src/drivers/wpb_home_apps/include/f_fwh_script.h<|end_filename|>
#ifndef INNO_SCRIPT_H
#define INNO_SCRIPT_H
#include "action_manager.h"

class FwhScript : public CActionManager
{
public:
	FwhScript();
	~FwhScript();
    void Queue();
};

#endif // INNO_SCRIPT_H


<|start_filename|>robot/src/fwh_sound/src/fwh_sound_yz.cpp<|end_filename|>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt32.h>

static ros::Publisher spk_pub;
static ros::Publisher ins_pub;
static std_msgs::String speaker_str;
static std_msgs::String ins_str;
static bool ifdoing = false;

void sound_check(const std_msgs::String::ConstPtr & msg){
    if (ifdoing == false) {
        printf("[识别到的语音] - %s \n",msg->data.c_str());
        int num = 0;
        num = msg->data.find("启动目标识别");
        if( num >= 0 ){
            speaker_str.data = "收到，正在启动目标识别";
            spk_pub.publish(speaker_str);
            ins_str.data = "obj";
            ins_pub.publish(ins_str);
            ifdoing = true;
        }
        num = msg->data.find("迷路");
        num = num + msg->data.find("启动导航");
        if( num >= -1 ){
            speaker_str.data = "收到，正在启动导航";
            spk_pub.publish(speaker_str);
            ins_str.data = "navi";
            ins_pub.publish(ins_str);
            ifdoing = true;
        }
        num = msg->data.find("控制");
        if( num >= 0 ){
            speaker_str.data = "收到，正在启动控制功能";
            spk_pub.publish(speaker_str);
            ins_str.data = "mani";
            ins_pub.publish(ins_str);
            ifdoing = true;
        }
        num = msg->data.find("跟着我");
        num = num + msg->data.find("跟随");
        if( num >= -1 ){
            speaker_str.data = "收到，正在启动跟随功能";
            spk_pub.publish(speaker_str);
            ins_str.data = "follow";
            ins_pub.publish(ins_str);
            ifdoing = true;
        }
        num = msg->data.find("建图");
        num = num + msg->data.find("建立地图");
        if( num >= -1){
            speaker_str.data = "收到，正在启动建图功能";
            spk_pub.publish(speaker_str);
            ins_str.data = "build";
            ins_pub.publish(ins_str);
            ifdoing = true;
        }
    }
    else {
        speaker_str.data = "稍等，正在执行前一条指令";
        spk_pub.publish(speaker_str);
    }
}

void state_change(const std_msgs::UInt32::ConstPtr & msg){
    ROS_WARN("finish get!");
    ifdoing=false;
    speaker_str.data = "执行完毕，可以输入下一条指令";
    spk_pub.publish(speaker_str);
}

int main(int argc, char** argv){

    ros::init(argc, argv, "fwh_sound_yz");
    ROS_WARN("fwh_sound_yz start!");
    ros::NodeHandle nh;
    ros::Subscriber ifdo = nh.subscribe("/fight_with_hair/uito/state_sound", 1, state_change);
    ins_pub = nh.advertise<std_msgs::String>("/fight_with_hair/toui/sound_ins", 10);
    ros::Subscriber sound = nh.subscribe("/xfyun/iat", 10, sound_check);
    spk_pub = nh.advertise<std_msgs::String>("/xfyun/tts", 10);
    ros::spin();
    return 0;
}


<|start_filename|>robot/src/drivers/waterplus_map_tools/include/add_waypoint_tool.h<|end_filename|>
#ifndef ADD_WAYPOINT_TOOL_H
#define ADD_WAYPOINT_TOOL_H

#ifndef Q_MOC_RUN
#include <QObject>
#include <ros/ros.h>
#include "rviz/default_plugin/tools/pose_tool.h"
#endif

namespace rviz
{
    class Arrow;
    class DisplayContext;
    class StringProperty;

    class AddWaypointTool: public rviz::PoseTool
    {
        Q_OBJECT
        public:
        AddWaypointTool();
        ~AddWaypointTool();

        virtual void onInitialize();

        protected:
        virtual void onPoseSet(double x, double y, double theta);

        private Q_SLOTS:
        void updateTopic();

        private:
        ros::NodeHandle nh_;
        ros::Publisher pub_;
        ros::ServiceClient cliGetWPName;
        StringProperty* topic_property_;
    };

}

#endif // ADD_WAYPOINT_TOOL_H

<|start_filename|>robot/src/fight_with_hair_ui/src/build_map_auto.cpp<|end_filename|>
#include "../include/fight_with_hair_ui/build_map_auto.h"
#include "ui_build_map_auto.h"
#include <QMessageBox>
#include <QKeyEvent>
#include <QString>
#include <stdio.h>
#include <string>
#include <QAbstractItemView>
#include <QCloseEvent>

build_map_auto::build_map_auto(bool isSim,QWidget *parent) :
  QWidget(parent),
  ui(new Ui::build_map_auto)
{
  ui->setupUi(this);

  if(isSim){
    QString autoMap="gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch rrt_slam rrt_slam_sim.launch'&";
    system(autoMap.toLatin1().data());
  }else{
    QString autoMap="gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch rrt_slam rrt_slam.launch'&";
    system(autoMap.toLatin1().data());
  }


  ui->tableWidget->setEditTriggers(QAbstractItemView::NoEditTriggers);
  ui->tableWidget->horizontalHeader()->setStretchLastSection(true);
  render_panel_ = new rviz::RenderPanel;
  ui->mapdisplay_layout->addWidget(render_panel_);
  manager_ = new rviz::VisualizationManager(render_panel_);
  tool_manager_ = manager_->getToolManager();
  render_panel_->initialize(manager_->getSceneManager(), manager_);
  manager_->initialize();
  manager_->startUpdate();
}

build_map_auto::~build_map_auto()
{
  delete ui;
}

void build_map_auto::on_add_map_button_clicked()
{
  static int row=0;
  std::string str=std::to_string(row);
  const char *p = str.data();
  //    itoa(row,p,10);
  QString mapName=ui->input->text();
  ui->tableWidget->insertRow(ui->tableWidget->rowCount());
  ui->tableWidget->setItem(row, 0, new QTableWidgetItem(QString(p)));
  ui->tableWidget->setItem(row, 1, new QTableWidgetItem(mapName));
  row++;
  QString s1="gnome-terminal -x bash -c 'source ~/.bashrc;rosrun map_server map_saver -f ~/fight_with_hair/src/navi/maps/"+mapName+"'&";
  system(s1.toLatin1().data());
  QMessageBox::information(this, tr("提示"), tr("保存成功!"));
  hasSaved = true;
}

void build_map_auto::on_returnMain_clicked()
{
  QString string;
  if(!hasSaved){
    QMessageBox::StandardButton choice=QMessageBox::question(this,"Warning","地图尚未保存，确定退出建图界面？",QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes);
    if(choice==QMessageBox::Yes){
//      system(string.toLatin1().data());
      QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
      system(killString.toLatin1().data());
      killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
      system(killString.toLatin1().data());
      
      this->close();
    }
    return ;
  }
//  system(string.toLatin1().data());
  QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
  system(killString.toLatin1().data());
  killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
  system(killString.toLatin1().data());
  this->close();
}

void build_map_auto::on_logout_clicked()
{
  QMessageBox::StandardButton choice=QMessageBox::question(this,"确认","确定要退出吗？",QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes);
  if(choice==QMessageBox::Yes){
    this->close();
//    QString string;
//    system(string.toLatin1().data());
//    string;
//    system(string.toLatin1().data());
    QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
    system(killString.toLatin1().data());
    killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
    system(killString.toLatin1().data());
    QApplication::quit();
  }
}

void build_map_auto::on_addNaviGoal_clicked()
{
  current_tool_ = tool_manager_->addTool("rviz/SetGoal");
  rviz::Property *pro = current_tool_->getPropertyContainer();
  pro->subProp("Topic")->setValue("move_base_simple/goal");
  manager_->setFixedFrame("map");
  tool_manager_->setCurrentTool(current_tool_);
}

void build_map_auto::on_addPublishPoint_clicked()
{
  current_tool_ = tool_manager_->addTool("rviz/PublishPoint");
  rviz::Property *pro = current_tool_->getPropertyContainer();
  pro->subProp("Topic")->setValue("/clicked_point");
  manager_->setFixedFrame("map");
  tool_manager_->setCurrentTool(current_tool_);
}

void build_map_auto::on_map_show_btn_clicked()
{

  manager_->removeAllDisplays();

  rviz::Display *map_=manager_->createDisplay("rviz/Map","fwh_ui map",true);
  ROS_ASSERT(map_!=NULL);
  map_->subProp("Topic")->setValue("/map");
  map_->subProp("Color Scheme")->setValue("map");

  rviz::Display *map2_=manager_->createDisplay("rviz/Map","fwh_ui map2",true);
  ROS_ASSERT(map2_!=NULL);
  map2_->subProp("Topic")->setValue("/move_base_node/global_costmap/costmap");
  map2_->subProp("Color Scheme")->setValue("costmap");



  rviz::Display *robot_=manager_->createDisplay("rviz/RobotModel","fwh_ui robot",true);
  ROS_ASSERT(robot_!=NULL);
  robot_->subProp("Robot Description")->setValue("robot_description");

  rviz::Display *laser_=manager_->createDisplay("rviz/LaserScan","fwh_ui laser",true);
  ROS_ASSERT(laser_!=NULL);
  laser_->subProp("Topic")->setValue("/scan");
  laser_->subProp("Size (m)")->setValue("0.1");


  rviz::Display *path1_=manager_->createDisplay("rviz/Path","fwh_ui_path_1",true);
  ROS_ASSERT(path1_!=NULL);
  path1_->subProp("Topic")->setValue("/move_base_node/NavfnROS/plan");
  path1_->subProp("Line Style")->setValue("Lines");
  path1_->subProp("Color")->setValue("25; 255; 0");
  path1_->subProp("Alpha")->setValue("1");
  path1_->subProp("Buffer Length")->setValue("1");

  rviz::Display *path2_=manager_->createDisplay("rviz/Path","fwh_ui_path_2",true);
  ROS_ASSERT(path2_!=NULL);
  path2_->subProp("Topic")->setValue("/move_base_node/TrajectoryPlannerROS/global_plan");
  path2_->subProp("Line Style")->setValue("Lines");
  path2_->subProp("Color")->setValue("85; 0; 127");
  path2_->subProp("Alpha")->setValue("1");
  path2_->subProp("Buffer Length")->setValue("1");


  rviz::Display *path3_=manager_->createDisplay("rviz/Path","fwh_ui_path_3",true);
  ROS_ASSERT(path3_!=NULL);
  path3_->subProp("Topic")->setValue("/move_base_node/TrajectoryPlannerROS/local_plan");
  path3_->subProp("Line Style")->setValue("Lines");
  path3_->subProp("Color")->setValue("255; 255; 127");
  path3_->subProp("Alpha")->setValue("1");
  path3_->subProp("Buffer Length")->setValue("1");


  rviz::Display *marker1_=manager_->createDisplay("rviz/Marker","fwh_ui_marker_1",true);
  ROS_ASSERT(marker1_!=NULL);
  marker1_->subProp("Marker Topic")->setValue("/global_detector_shapes");
  marker1_->subProp("Queue Size")->setValue("100");

  rviz::Display *marker2_=manager_->createDisplay("rviz/Marker","fwh_ui_marker_2",true);
  ROS_ASSERT(marker2_!=NULL);
  marker2_->subProp("Marker Topic")->setValue("/local_detector_shapes");
  marker2_->subProp("Queue Size")->setValue("100");

  rviz::Display *marker3_=manager_->createDisplay("rviz/Marker","fwh_ui_marker_3",true);
  ROS_ASSERT(marker3_!=NULL);
  marker3_->subProp("Marker Topic")->setValue("/centroids");
  marker3_->subProp("Queue Size")->setValue("100");

  manager_->startUpdate();
}


void build_map_auto::setMain(fight_with_hair_ui::MainWindow *m){
  main=m;
}


void build_map_auto::closeEvent(QCloseEvent *event){
  main->show();
  emit build_map_auto_finish();
}


<|start_filename|>robot/src/drivers/xfyun_waterplus/src/asr_node.cpp<|end_filename|>
/* Copiright belong to xfyun(IFLYTEK CO.,LTD)                            */
/* @modified by <NAME>                                             */
#include <ros/ros.h>
#include <std_msgs/String.h>

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>

#include "qisr.h"
#include "msp_cmn.h"
#include "msp_errors.h"

#define	BUFFER_SIZE 2048
#define HINTS_SIZE  100
#define GRAMID_LEN	128
#define FRAME_LEN	640 

static ros::Publisher asr_pub;
static char*		grammar_id		=	NULL;

int get_grammar_id(std::string grammar_file, char* grammar_id, unsigned int id_len)
{
	FILE*			fp				=	NULL;
	char*			grammar			=	NULL;
	unsigned int	grammar_len		=	0;
	unsigned int	read_len		=	0;
	const char*		ret_id			=	NULL;
	unsigned int	ret_id_len		=	0;
	int				ret				=	-1;	

	if (NULL == grammar_id)
		goto grammar_exit;

	fp = fopen(grammar_file.c_str(), "rb");
	if (NULL == fp)
	{   
		printf("\nopen grammar file failed!\n");
		goto grammar_exit;
	}
	
	fseek(fp, 0, SEEK_END);
	grammar_len = ftell(fp); //获取语法文件大小 
	fseek(fp, 0, SEEK_SET); 

	grammar = (char*)malloc(grammar_len + 1);
	if (NULL == grammar)
	{
		printf("\nout of memory!\n");
		goto grammar_exit;
	}

	read_len = fread((void *)grammar, 1, grammar_len, fp); //读取语法内容
	if (read_len != grammar_len)
	{
		printf("\nread grammar error!\n");
		goto grammar_exit;
	}
	grammar[grammar_len] = '\0';

	ret_id = MSPUploadData("usergram", grammar, grammar_len, "dtt = abnf, sub = asr", &ret); //上传语法
	if (MSP_SUCCESS != ret)
	{
		printf("\nMSPUploadData failed, error code: %d.\n", ret);
		goto grammar_exit;
	}

	ret_id_len = strlen(ret_id);
	if (ret_id_len >= id_len)
	{
		printf("\nno enough buffer for grammar_id!\n");
		goto grammar_exit;
	}
	strncpy(grammar_id, ret_id, ret_id_len);
	printf("grammar_id: \"%s\" \n", grammar_id); //下次可以直接使用该ID，不必重复上传语法。

grammar_exit:
	if (NULL != fp)
	{
		fclose(fp);
		fp = NULL;
	}
	if (NULL!= grammar)
	{
		free(grammar);
		grammar = NULL;
	}
	return ret;
}

void run_asr(const char* audio_file, const char* params, char* grammar_id)
{
	const char*		session_id						= NULL;
	char			rec_result[BUFFER_SIZE]		 	= {'\0'};	
	char			hints[HINTS_SIZE]				= {'\0'}; //hints为结束本次会话的原因描述，由用户自定义
	unsigned int	total_len						= 0;
	int 			aud_stat 						= MSP_AUDIO_SAMPLE_CONTINUE;		//音频状态
	int 			ep_stat 						= MSP_EP_LOOKING_FOR_SPEECH;		//端点检测
	int 			rec_stat 						= MSP_REC_STATUS_SUCCESS;			//识别状态	
	int 			errcode 						= MSP_SUCCESS;

	FILE*			f_pcm 							= NULL;
	char*			p_pcm 							= NULL;
	long 			pcm_count 						= 0;
	long 			pcm_size 						= 0;
	long			read_size						= 0;

	if (NULL == audio_file)
		goto asr_exit;

	f_pcm = fopen(audio_file, "rb");
	if (NULL == f_pcm) 
	{
		printf("\nopen [%s] failed!\n", audio_file);
		goto asr_exit;
	}
	
	fseek(f_pcm, 0, SEEK_END);
	pcm_size = ftell(f_pcm); //获取音频文件大小 
	fseek(f_pcm, 0, SEEK_SET);		

	p_pcm = (char*)malloc(pcm_size);
	if (NULL == p_pcm)
	{
		printf("\nout of memory!\n");
		goto asr_exit;
	}

	read_size = fread((void *)p_pcm, 1, pcm_size, f_pcm); //读取音频文件内容
	if (read_size != pcm_size)
	{
		printf("\nread [%s] failed!\n", audio_file);
		goto asr_exit;
	}
	
	printf("\n开始语音识别 ...\n");
	session_id = QISRSessionBegin(grammar_id, params, &errcode);
	if (MSP_SUCCESS != errcode)
	{
		printf("\nQISRSessionBegin failed, error code:%d\n", errcode);
		goto asr_exit;
	}
	
	while (1) 
	{
		unsigned int len = 10 * FRAME_LEN; // 每次写入200ms音频(16k，16bit)：1帧音频20ms，10帧=200ms。16k采样率的16位音频，一帧的大小为640Byte
		int ret = 0;

		if (pcm_size < 2 * len) 
			len = pcm_size;
		if (len <= 0)
			break;
		
		aud_stat = MSP_AUDIO_SAMPLE_CONTINUE;
		if (0 == pcm_count)
			aud_stat = MSP_AUDIO_SAMPLE_FIRST;
		
		printf(">");
		ret = QISRAudioWrite(session_id, (const void *)&p_pcm[pcm_count], len, aud_stat, &ep_stat, &rec_stat);
		if (MSP_SUCCESS != ret)
		{
			printf("\nQISRAudioWrite failed, error code:%d\n",ret);
			goto asr_exit;
		}
			
		pcm_count += (long)len;
		pcm_size  -= (long)len;
		
		if (MSP_EP_AFTER_SPEECH == ep_stat)
			break;
		usleep(200*1000); //模拟人说话时间间隙，10帧的音频长度为200ms
	}
	errcode = QISRAudioWrite(session_id, NULL, 0, MSP_AUDIO_SAMPLE_LAST, &ep_stat, &rec_stat);
	if (MSP_SUCCESS != errcode)
	{
		printf("\nQISRAudioWrite failed, error code:%d\n",errcode);
		goto asr_exit;	
	}

	while (MSP_REC_STATUS_COMPLETE != rec_stat) 
	{
		const char *rslt = QISRGetResult(session_id, &rec_stat, 0, &errcode);
		if (MSP_SUCCESS != errcode)
		{
			printf("\nQISRGetResult failed, error code: %d\n", errcode);
			goto asr_exit;
		}
		if (NULL != rslt)
		{
			unsigned int rslt_len = strlen(rslt);
			total_len += rslt_len;
			if (total_len >= BUFFER_SIZE)
			{
				printf("\nno enough buffer for rec_result !\n");
				goto asr_exit;
			}
			strncat(rec_result, rslt, rslt_len);
		}
		usleep(150*1000); //防止频繁占用CPU
	}
	printf("\n语音识别结束\n");
	printf("=============================================================\n");
	printf("%s",rec_result);
	printf("=============================================================\n");

asr_exit:
	if (NULL != f_pcm)
	{
		fclose(f_pcm);
		f_pcm = NULL;
	}
	if (NULL != p_pcm)
	{	
		free(p_pcm);
		p_pcm = NULL;
	}

	QISRSessionEnd(session_id, hints);
}

static const char*	session_begin_params	=	"sub = asr, result_type = plain, result_encoding = utf8,sample_rate = 16000,aue = speex-wb";
void rec_file_callback(const std_msgs::String::ConstPtr& msg)
{
    const char* audio_file = msg->data.c_str();
	//ROS_INFO("[rec_file ] %s",audio_file);
    
	run_asr(audio_file, session_begin_params, grammar_id);
}

int main(int argc, char* argv[])
{
	ros::init(argc, argv, "xfyun_asr_node");
    ros::NodeHandle n;
    asr_pub = n.advertise<std_msgs::String>("/xfyun/asr", 20);
	ros::Subscriber sub =n.subscribe("/xfyun/reco_file",2,rec_file_callback);

	char const* home_dir = getenv("HOME");
	std::string strHomeDir = home_dir;
	std::string strFile = strHomeDir + "/catkin_ws/src/wpb_home_apps_2017/asr/1_follow.bnf";

    int			ret						=	MSP_SUCCESS;
	const char* login_params			=	"appid = 58eeeedf, work_dir = .";
    ret = MSPLogin(NULL, NULL, login_params); //第一个参数是用户名，第二个参数是密码，均传NULL即可，第三个参数是登录参数
	if (MSP_SUCCESS != ret)
	{
		printf("MSPLogin failed, error code: %d.\n",ret);
		goto exit; //登录失败，退出登录
	}


    grammar_id = (char*)malloc(GRAMID_LEN);
	if (NULL == grammar_id)
	{
		printf("out of memory !\n");
		goto exit;
	}
	memset(grammar_id, 0, GRAMID_LEN);

	printf("上传语法 ...\n");
	ret = get_grammar_id(strFile, grammar_id, GRAMID_LEN);
	if (MSP_SUCCESS != ret)
		goto exit;
	printf("上传语法成功\n");

    ros::spin();

exit:
	if (NULL != grammar_id)
	{
		free(grammar_id);
		grammar_id = NULL;
	}
	MSPLogout(); //退出登录
}

<|start_filename|>robot/src/fight_with_hair_ui/src/cctrldashboard.cpp<|end_filename|>
#include "../include/fight_with_hair_ui/cctrldashboard.h"
#include <QPainter>
#include <QDebug>
#include <qmath.h>
// add by lyh
CCtrlDashBoard::CCtrlDashBoard(QWidget *parent, StyleType type) :
    QWidget(parent),
    m_StyleType(type)
{
    m_BorderColor = QColor(60,60,60);
    m_BgColor = QColor(160,160,160);
    m_FrontColor = Qt::white;

    m_DashValue= 0;
    m_MinValue = -100;
    m_MaxValue = 100;
    m_DashNum = 10;
    m_LineLength=0;

    m_StartAngle=90;  //510
    m_EndAngle=0;   //120
    update();
}

void CCtrlDashBoard::drawBackGround(QPainter *painter, qreal hlafWidth)
{
    m_MaxBorderRadius = ((hlafWidth*5)/6);
    qreal startX=hlafWidth-m_MaxBorderRadius;
    painter->save();
    painter->setPen(m_BorderColor);
    painter->setBrush(m_BorderColor);

    QPainterPath bigCircle;
    bigCircle.addEllipse(startX, startX, (m_MaxBorderRadius*2), (m_MaxBorderRadius*2));

    m_MinBorderRadius = m_MaxBorderRadius-20;
    startX=hlafWidth-m_MinBorderRadius;
    QPainterPath smallCircle;
    smallCircle.addEllipse(startX, startX, (m_MinBorderRadius*2), (m_MinBorderRadius*2));

    QPainterPath path = bigCircle - smallCircle;
    painter->drawPath(path);
    painter->restore();

    painter->save();
    painter->setBrush(m_BgColor);
    painter->drawEllipse(startX,startX,(m_MinBorderRadius*2), (m_MinBorderRadius*2));
    if (m_Unit == 0)
      painter->drawText(startX+90,startX+170,"cm/S");
    else
      painter->drawText(startX+90,startX+170,"rad/S");
    painter->restore();
    m_DialsRadius = m_MinBorderRadius-10;
    if(m_DialsRadius < 0){
        m_DialsRadius=2;
    }
}
void CCtrlDashBoard::drawScaleDials(QPainter *painter, qreal hlafWidth)
{
    qreal tSteps = (m_MaxValue - m_MinValue)/m_DashNum; //相乘后的值是分的份数
    qreal angleStep = 1.0*(360.0-m_StartAngle - m_EndAngle) / tSteps; //每一个份数的角度

    painter->save();
    QPen pen ;
    pen.setColor(m_FrontColor); //推荐使用第二种方式
    painter->translate(hlafWidth,hlafWidth);
    painter->rotate(m_StartAngle);
    m_LineLength = (hlafWidth/16);
    qreal lineStart = m_DialsRadius-4*m_LineLength-m_LineLength;
    qreal lineSmStart = m_DialsRadius-4*m_LineLength-m_LineLength/2;
    qreal lineEnd = m_DialsRadius-4*m_LineLength;
    for (int i = 0; i <= tSteps; i++)
    {
        if (i % 10 == 0)//整数刻度显示加粗
        {
            pen.setWidth(2); //设置线宽
            painter->setPen(pen); //使用面向对象的思想，把画笔关联上画家。通过画家画出来
            painter->drawLine(lineStart,lineStart, lineEnd, lineEnd); //两个参数应该是两个坐标值
            //painter->drawText(lineEnd+6,lineEnd+6, tr("%1").arg(m_MinValue+i));
        }
        else
        {
            pen.setWidth(1);
            painter->setPen(pen);
            painter->drawLine(lineSmStart, lineSmStart, lineEnd, lineEnd); //两个参数应该是两个坐标值
        }
        painter->rotate(angleStep);
    }
    painter->restore();

    painter->save();
    painter->setPen(pen);
    //m_startAngle是起始角度，m_endAngle是结束角度，m_scaleMajor在一个量程中分成的刻度数
    qreal startRad = ( 315-m_StartAngle) * (3.14 / 180);
    qreal deltaRad = (360-m_StartAngle - m_EndAngle) * (3.14 / 180) / tSteps;
    qreal sina,cosa;
    qreal x, y;
    QFontMetricsF fm(this->font());
    double w, h, tmpVal;
    QString str;
    painter->translate(hlafWidth,hlafWidth);
    lineEnd = m_MinBorderRadius-8;
    for (int i = 0; i <= tSteps; i++)
    {
        if (i % 10 == 0)//整数刻度显示加粗
        {
            sina = qSin(startRad - i * deltaRad);
            cosa = cos(startRad - i * deltaRad);

            tmpVal = 1.0 * i *((m_MaxValue - m_MinValue) / tSteps) + m_MinValue;
            str = QString( "%1" ).arg(tmpVal);  //%1作为占位符   arg()函数比起 sprintf()来是类型安全的
            w = fm.size(Qt::TextSingleLine,str).width();
            h = fm.size(Qt::TextSingleLine,str).height();
            x = lineEnd * cosa - w / 2;
            y = -lineEnd * sina + h / 4;
            painter->drawText(x, y, str); //函数的前两个参数是显示的坐标位置，后一个是显示的内容，是字符类型""
        }
    }
    painter->restore();
}

void CCtrlDashBoard::drawIndicator(QPainter *painter, qreal hlafWidth)
{
    QPolygon pts;
    pts.setPoints(3, -8,0, 8,0, 0,(int)m_DialsRadius-20); /* (-2,0)/(2,0)/(0,60) *///第一个参数是 ，坐标的个数。后边的是坐标
    painter->save();
    painter->translate(hlafWidth, hlafWidth);
    painter->rotate(m_StartAngle-45);

    if(m_DashValue>0){
        qreal tSteps = (m_MaxValue - m_MinValue)/m_DashNum; //相乘后的值是分的份数
        qreal angleStep = 1.0*(360.0-m_StartAngle - m_EndAngle) / tSteps; //每一个份数的角度
        double degRotate = angleStep*tSteps*(m_DashValue/100.0)/*+angleStep*/;

        //画指针
        //qDebug() <<"degRotate =="<<degRotate<<"m_DashValue =="<<m_DashValue<<m_StartAngle<<m_DialsRadius<<hlafWidth;
        if(m_DashValue == 99){
            painter->rotate(m_EndAngle-m_StartAngle);  //顺时针旋转坐标系统
        }else
        {
            painter->rotate(degRotate);  //顺时针旋转坐标系统
        }

    }
    QRadialGradient haloGradient(0, 0, hlafWidth/2, 0, 0);  //辐射渐变
    haloGradient.setColorAt(0, QColor(255,69,0));
    haloGradient.setColorAt(1, QColor(255,0,0));
    painter->setPen(Qt::white); //定义线条文本颜色  设置线条的颜色
    painter->setBrush(haloGradient);//刷子定义形状如何填满 填充后的颜色
    painter->drawConvexPolygon(pts); //这是个重载函数，绘制多边形。
    painter->restore();

    //画中心点
    QColor niceBlue(150, 150, 200);
    QConicalGradient coneGradient(0, 0, -90.0);  //角度渐变
    coneGradient.setColorAt(0.0, Qt::darkGray);
    coneGradient.setColorAt(0.2, niceBlue);
    coneGradient.setColorAt(0.5, Qt::white);
    coneGradient.setColorAt(1.0, Qt::darkGray);
    painter->save();
    painter->translate(hlafWidth,hlafWidth);
    painter->setPen(Qt::NoPen);  //没有线，填满没有边界
    painter->setBrush(coneGradient);
    painter->drawEllipse(-10, -10, 20, 20);
    painter->restore();
}

void CCtrlDashBoard::drawIndicatorBar(QPainter *painter, qreal hlafWidth)
{
    // 渐变色
    painter->save();
    painter->translate(hlafWidth,hlafWidth);
    qreal lineEnd = m_DialsRadius-3*m_LineLength;
    QRadialGradient gradient(0, 0, lineEnd);
    gradient.setColorAt(0, Qt::white);
    gradient.setColorAt(1.0, Qt::blue);
    painter->setBrush(gradient);

    // << 1（左移1位）相当于radius*2 即：150*2=300
    //QRectF(-150, -150, 300, 300)
    QRectF rect(-lineEnd, -lineEnd, lineEnd*2, lineEnd*2);
    QPainterPath path;
    path.arcTo(rect, m_StartAngle, 270);

    // QRectF(-120, -120, 240, 240)
    QPainterPath subPath;
    subPath.addEllipse(rect.adjusted(10, 10, -10, -10));

    // path为扇形 subPath为椭圆
    path -= subPath;
    painter->setPen(Qt::NoPen);
    painter->rotate(m_StartAngle+45);
    painter->drawPath(path);
    painter->restore();


    qreal degRotate=0.1;
    if(m_DashValue>0){
        qreal tSteps = (m_MaxValue - m_MinValue)/m_DashNum; //相乘后的值是分的份数
        qreal angleStep = 1.0*(360.0-m_StartAngle - m_EndAngle) / tSteps; //每一个份数的角度
        degRotate = angleStep*tSteps*(m_DashValue/100.0)/*+angleStep*/;

        //画指针
        //qDebug() <<"degRotate =="<<degRotate<<"m_DashValue =="<<m_DashValue<<m_StartAngle<<m_DialsRadius<<hlafWidth;
        /*if(m_DashValue == 99){
            painter->rotate(m_EndAngle-m_StartAngle);  //顺时针旋转坐标系统
        }else
        {
            painter->rotate(degRotate);  //顺时针旋转坐标系统
        }*/
    }

    painter->save();
    painter->translate(hlafWidth, hlafWidth);

    QRadialGradient ftGradient(0, 0, lineEnd);
    ftGradient.setColorAt(0, Qt::white);
    ftGradient.setColorAt(1.0, Qt::darkYellow);
    painter->setBrush(ftGradient);

    // << 1（左移1位）相当于radius*2 即：150*2=300
    //QRectF(-150, -150, 300, 300)
    QRectF ftRect(-lineEnd, -lineEnd, lineEnd*2, lineEnd*2);
    QPainterPath ftPath;
    ftPath.arcTo(ftRect, m_EndAngle-m_StartAngle, -degRotate);
    // path为扇形 subPath为椭圆
    ftPath -= subPath;
    painter->rotate(m_StartAngle-45);
    painter->drawPath(ftPath);
    painter->restore();

    QPolygon pts;
    int pointLength=lineEnd-12;
    pts.setPoints(3, -6,pointLength-10, 6,pointLength-10, 0,pointLength);
    painter->save();
    painter->translate(hlafWidth, hlafWidth);
    painter->rotate(m_StartAngle-45);

    if(m_DashValue == 99){
        painter->rotate(m_EndAngle-m_StartAngle);  //顺时针旋转坐标系统
    }else
    {
        painter->rotate(degRotate);  //顺时针旋转坐标系统
    }
    painter->setPen(Qt::white); //定义线条文本颜色  设置线条的颜色
    painter->setBrush(Qt::blue);//刷子定义形状如何填满 填充后的颜色
    painter->drawConvexPolygon(pts); //这是个重载函数，绘制多边形。
    painter->restore();
}

void CCtrlDashBoard::paintEvent(QPaintEvent * event)
{
    QPainter p(this);
    qreal width = qMin((this->width()>>1), (this->height()>>1));

    p.setRenderHints(QPainter::Antialiasing|QPainter::TextAntialiasing);

    drawBackGround(&p, width);

    drawScaleDials(&p, width);
    switch(m_StyleType)
    {
    case ST_DEFAULT:
        drawIndicator(&p, width);
        break;
    case ST_ARCBAR:
        drawIndicatorBar(&p, width);
        break;
    }
}


<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/usercontrol.h<|end_filename|>
#ifndef USERCONTROL_H
#define USERCONTROL_H

#include <QWidget>
#include <rviz/visualization_manager.h>
#include <rviz/render_panel.h>
#include <rviz/display.h>
#include <rviz/tool_manager.h>
#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>
#include <rviz/tool.h>
#include <fight_with_hair_ui/main_window.hpp>
#include "cctrldashboard.h"


namespace Ui {
class userControl;
}

class userControl : public QWidget
{
  Q_OBJECT

public:
  explicit userControl(QWidget *parent = 0);
  ~userControl();

  void setMain(fight_with_hair_ui::MainWindow *);
  void sendDataInit();

Q_SIGNALS:
  void movemsg(float,float,float);
  void armmsg(float,float,float,float);
  void userControl_finish();

private slots:
  void on_logout_clicked();

  void on_returnMain_clicked();

  void on_forward_clicked();

  void on_left_clicked();

  void on_backward_clicked();

  void on_right_clicked();

  void on_stop_clicked();

  void on_left_forward_clicked();

  void on_right_forward_clicked();

  void on_left_back_clicked();

  void on_right_back_clicked();

  void on_up_clicked();

  void on_down_clicked();

  void on_grab_clicked();

  void on_release_clicked();

  void on_open_rviz_clicked();

  void showSpeed(float,float,float);

  void on_open_rviz_image_clicked();

  void on_recover_clicked();

private:
  Ui::userControl *ui;
  rviz::VisualizationManager *manager_;
  rviz::RenderPanel *render_panel_;
  fight_with_hair_ui::MainWindow *main;
  CCtrlDashBoard *x_speed;
  CCtrlDashBoard *y_speed;
  CCtrlDashBoard *z_speed;
  float x_send=0.2;
  float y_send=0;
  float z_send=0.5;
  bool clickForward = false;
  bool clickBack = false;
  bool clickTurnLeft = false;
  bool clickTurnRight = false;

protected:
  void keyPressEvent(QKeyEvent *ev);
  void keyReleaseEvent(QKeyEvent *ev);
  void closeEvent(QCloseEvent *event);
};

#endif // USERCONTROL_H


<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/loadWayPoint.h<|end_filename|>
#pragma once

#include <tinyxml.h>
#include <vector>
#include <iostream>
#include <cstring>
#include <cstdlib>

struct WayPoint {
    std::string name;
    float pos_x, pos_y, pos_z;
    float ori_x, ori_y, ori_z, ori_w;
    
    WayPoint() {
        name = "";
        pos_x = pos_y = pos_z = 0;
        ori_x = ori_y = ori_z = ori_w = 0;
    }
};

std::vector<WayPoint> LoadWaypointsFromFile(std::string inFilename);

<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/build_map.h<|end_filename|>
#ifndef BUILD_MAP_H
#define BUILD_MAP_H

#include <QWidget>
#include <rviz/visualization_manager.h>
#include <rviz/render_panel.h>
#include <rviz/display.h>
#include <rviz/tool_manager.h>

#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>
#include <fight_with_hair_ui/main_window.hpp>

namespace Ui {
class build_map;
}

class build_map : public QWidget
{
  Q_OBJECT

public:
  explicit build_map(bool isSim,QWidget *parent = 0);
  ~build_map();

  void ros_init(int argc,char **argv);

  void setMain(fight_with_hair_ui::MainWindow*);

  void setEnvironment(bool);


Q_SIGNALS:
  void movemsg(float,float,float);
  void build_map_finish();

private slots:
  void on_logout_clicked();

  void on_returnMain_clicked();

  void on_add_map_button_clicked();

  void on_map_show_btn_clicked();

  void on_forward_clicked();

  void on_right_forward_clicked();

  void on_right_clicked();

  void on_right_back_clicked();

  void on_backward_clicked();

  void on_left_back_clicked();

  void on_left_clicked();

  void on_left_forward_clicked();

  void on_stop_clicked();

private:
  Ui::build_map *ui;
  rviz::VisualizationManager *manager_;
  rviz::RenderPanel *render_panel_;
  rviz::Display *map_;
  QTimer *m_timer;
  bool hasSaved=false;
  fight_with_hair_ui::MainWindow *main;
  bool isSim=false;

protected:
  void keyPressEvent(QKeyEvent *ev);
  void keyReleaseEvent(QKeyEvent *ev);
  void closeEvent(QCloseEvent *event);
};

#endif // BUILD_MAP_H


<|start_filename|>robot/src/fight_with_hair_ui/src/spinslider.cpp<|end_filename|>
#include "../include/fight_with_hair_ui/spinslider.h"
#include "ui_spinslider.h"

spinslider::spinslider(QWidget *parent) :
  QWidget(parent),
  ui(new Ui::spinslider)
{
  ui->setupUi(this);
  DashBoard_x = new CCtrlDashBoard(ui->speed_x_widget);
  DashBoard_x->setGeometry(ui->speed_x_widget->rect());
  DashBoard_x->setValue(0);
  DashBoard_x->setUnit(0);
}

spinslider::~spinslider()
{
  delete ui;
}


<|start_filename|>robot/src/fwh_obj/src/fwh_obj_grab.cpp<|end_filename|>
#include <ros/ros.h>
#include <fwh_obj/tarobj.h>
#include <geometry_msgs/Pose.h>


static ros::Publisher grab_pub;

void grab(const fwh_obj::tarobj::ConstPtr &msg){
    ROS_WARN("target's location is (%.2f , %.2f , %.2f)",msg->x,msg->y,msg->z);
    geometry_msgs::Pose grab_msg;
    grab_msg.position.x = 0.99;
    grab_msg.position.y = -0.16;
    grab_msg.position.z = 1.00;
    grab_pub.publish(grab_msg);
 
}


int main(int argc, char** argv){
    
    ros::init(argc, argv, "fwh_obj_grab");
    ROS_WARN("fwh_obj_grab start!");
    ros::NodeHandle nh;
    ros::Subscriber tar_sub = nh.subscribe("/tograb/obj_target", 1, grab);
    grab_pub = nh.advertise<geometry_msgs::Pose>("/wpb_home/grab_action", 1);
    ros::spin();
    return 0;

}


<|start_filename|>robot/src/drivers/wpb_home_apps/src/scripts/innovation_script.cpp<|end_filename|>
#include "8_innovation_script.h"
CInnovationScript::CInnovationScript()
{
    
}

CInnovationScript::~CInnovationScript()
{

}

void CInnovationScript::Queue()
{
    stAct newAct;
   
    newAct.nAct = ACT_LISTEN;
    newAct.strTarget = "拿杯水";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "好的,请稍等";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_ADD_WAYPOINT;
    newAct.strTarget = "master";
    arAct.push_back(newAct);

    newAct.nAct = ACT_GOTO;
    newAct.strTarget = "target";
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "即将开始抓取";
    newAct.nDuration = 6;
    arAct.push_back(newAct);

    newAct.nAct = ACT_GRAB;
    newAct.strTarget = "";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "物品抓取完毕";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_GOTO;
    newAct.strTarget = "master";
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "物品已取回，请接收";
    newAct.nDuration = 5;
    arAct.push_back(newAct);


    newAct.nAct = ACT_LISTEN;
    newAct.strTarget = "给我";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_PASS;
    newAct.strTarget = "";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "任务完成";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

}

<|start_filename|>robot/src/fight_with_hair_ui/src/loadWayPoint.cpp<|end_filename|>
#include "../include/fight_with_hair_ui/loadWayPoint.h"



std::vector<WayPoint> LoadWaypointsFromFile(std::string inFilename)
{
    TiXmlDocument docLoad(inFilename);
    std::vector<WayPoint> arWaypoint;
    bool resLoad = docLoad.LoadFile();
    if(resLoad == false)
    {

        return arWaypoint;
    }
    
    TiXmlElement* RootElement = docLoad.RootElement();
    for(TiXmlNode* item = RootElement->FirstChild("Waypoint");item;item = item->NextSibling("Waypoint"))
    {
        TiXmlNode* child = item->FirstChild();
        const char* name = child->ToElement()->GetText();

        WayPoint newWayPoint;
        newWayPoint.name = std::string(name); 
        child = item->IterateChildren(child);
        const char* pos_x = child->ToElement()->GetText();
        newWayPoint.pos_x = std::atof(pos_x);
        child = item->IterateChildren(child);
        const char* pos_y = child->ToElement()->GetText();
        newWayPoint.pos_y = std::atof(pos_y);
        child = item->IterateChildren(child);
        const char* pos_z = child->ToElement()->GetText();
        newWayPoint.pos_z = std::atof(pos_z);
        child = item->IterateChildren(child);
        const char* ori_x = child->ToElement()->GetText();
        newWayPoint.ori_x = std::atof(ori_x);
        child = item->IterateChildren(child);
        const char* ori_y = child->ToElement()->GetText();
        newWayPoint.ori_y = std::atof(ori_y);
        child = item->IterateChildren(child);
        const char* ori_z = child->ToElement()->GetText();
        newWayPoint.ori_z = std::atof(ori_z);
        child = item->IterateChildren(child);
        const char* ori_w = child->ToElement()->GetText();
        newWayPoint.ori_w = std::atof(ori_w);
        arWaypoint.push_back(newWayPoint);
    }


    return arWaypoint;
}


<|start_filename|>robot/src/navi/src/way_saver.cpp<|end_filename|>
// @author <NAME>

#include <ros/ros.h>
#include <waterplus_map_tools/SaveWaypoints.h>
#include <unistd.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "way_saver");

    ros::NodeHandle nh;
    ros::ServiceClient cliSave = nh.serviceClient<waterplus_map_tools::SaveWaypoints>("/waterplus/save_waypoints");
    waterplus_map_tools::SaveWaypoints srvS;

    std::string strSaveFile;
    char const *home = getenv("HOME");
    strSaveFile = argv[1];
    srvS.request.filename = strSaveFile;

    for (int i = 1; i < argc; i++)
    {
        if (!strcmp(argv[i], "-f"))
        {
            if (++i < argc)
            {
                srvS.request.filename = argv[i];
            }
        }
    }

    if (cliSave.call(srvS))
    {
        ROS_INFO("Save waypoints to the file : %s", srvS.request.filename.c_str());
    }
    else
    {
        ROS_ERROR("Failed to call service save_waypoints");
    }

    return 0;
}


<|start_filename|>robot/src/drivers/wpb_home_apps/src/person_recognition.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*!******************************************************************
 @author     ZhangWanjie
 ********************************************************************/

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/objdetect/objdetect.hpp"
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include "xfyun_waterplus/IATSwitch.h"
#include <sound_play/SoundRequest.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>

using namespace cv;
using namespace std;

static std::string rgb_topic;
static std::string pc_topic;
static std::string face_cascade_name;
static CascadeClassifier face_cascade;

static Mat frame_gray;
static ros::Publisher image_pub;
static std::vector<Rect> faces;
static std::vector<cv::Rect>::const_iterator face_iter;
static ros::Publisher ctrl_pub;
static std_msgs::String ctrl_msg;
static geometry_msgs::Pose2D pose_diff;

static ros::Publisher pc_pub;
static tf::TransformListener *tf_listener; 
static ros::Publisher marker_pub;
static visualization_msgs::Marker line_face;
static visualization_msgs::Marker pos_face;
static visualization_msgs::Marker text_marker;
static ros::Publisher spk_pub;
static ros::ServiceClient clientIAT;
static xfyun_waterplus::IATSwitch srvIAT;
static ros::Publisher vel_pub;

#define STATE_READY     0
#define STATE_OPERATOR  1
#define STATE_COUNTDOWN 2
#define STATE_TURN      3
#define STATE_CROWD     4
#define STATE_REPORT    5
#define STATE_DONE      6

static int nState = STATE_READY;
static int nCountDown = 300;
static int nTurnCount = 6;

//框选出人脸
cv::Mat drawFacesRGB(cv::Mat inImage) 
{
    std::vector<cv::Rect>::const_iterator i;
    for (face_iter = faces.begin(); face_iter != faces.end(); ++face_iter) 
    {
        cv::rectangle(
            inImage,
            cv::Point(face_iter->x , face_iter->y),
            cv::Point(face_iter->x + face_iter->width, face_iter->y + face_iter->height),
            CV_RGB(255, 0 , 255),
            10);
    }
    return inImage;
}

void DrawTextMarker(std::string inText, int inID, float inScale, float inX, float inY, float inZ, float inR, float inG, float inB)
{
    text_marker.header.frame_id = "base_footprint";
    text_marker.ns = "line_obj";
    text_marker.action = visualization_msgs::Marker::ADD;
    text_marker.id = inID;
    text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    text_marker.scale.z = inScale;
    text_marker.color.r = inR;
    text_marker.color.g = inG;
    text_marker.color.b = inB;
    text_marker.color.a = 1.0;

    text_marker.pose.position.x = inX;
    text_marker.pose.position.y = inY;
    text_marker.pose.position.z = inZ;
    
    text_marker.pose.orientation=tf::createQuaternionMsgFromYaw(1.0);

    text_marker.text = inText;

    marker_pub.publish(text_marker);
}

static void Speak(string inStr)
{
    sound_play::SoundRequest sp;
    sp.sound = sound_play::SoundRequest::SAY;
    sp.command = sound_play::SoundRequest::PLAY_ONCE;
    sp.arg = inStr;
    sp.volume = 1.0f;  //indigo(Ubuntu 14.04)需要注释掉这一句才能编译
    spk_pub.publish(sp);
}

void callbackRGB(const sensor_msgs::ImageConstPtr& msg)
{
    if(nState != STATE_OPERATOR && nState != STATE_CROWD)
    {
        return;
    }
    //ROS_INFO("callbackRGB");
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    // change contrast: 0.5 = half  ; 2.0 = double
    cv_ptr->image.convertTo(frame_gray, -1, 1.5, 0);

    // create B&W image
    cvtColor( frame_gray, frame_gray, CV_BGR2GRAY );

	equalizeHist( frame_gray, frame_gray );
    //-- Detect faces
	face_cascade.detectMultiScale( frame_gray, faces, 1.1, 9, 0|CV_HAAR_SCALE_IMAGE, Size(30, 30) );

    //ROS_INFO("face = %d",faces.size());
    if(faces.size() > 0)
    {
        cv_ptr->image = drawFacesRGB(cv_ptr->image);
    }

    image_pub.publish(cv_ptr->toImageMsg());

    int nNumFace = faces.size();
    if(nNumFace > 0)
    {
        if( nState == STATE_OPERATOR )
        {
            imwrite("/home/robot/operator.jpg",cv_ptr->image);
            ROS_INFO("Save the image of operator!!");
            ROS_INFO("Ready to turn!");
            Speak("OK,I have memoried you.Please go to the crowd.I will find you out.");
            usleep(7*1000*1000);
            nState = STATE_COUNTDOWN;
        }
        if(nState == STATE_CROWD)
        {
            imwrite("/home/robot/crowd.jpg",cv_ptr->image);
            ROS_INFO("Save the image of crowd!!");
            Speak("OK,I have recognize the faces of crowd.");
            nState = STATE_REPORT;
        }
    }
}

void callbackPointCloud(const sensor_msgs::PointCloud2 &input)
{
    if(nState != STATE_OPERATOR && nState != STATE_CROWD)
    {
        return;
    }
    //to footprint
    sensor_msgs::PointCloud2 pc_footprint;
    bool res = tf_listener->waitForTransform("/base_footprint", input.header.frame_id, input.header.stamp, ros::Duration(5.0)); 
    if(res == false)
    {
        return;
    }
    pcl_ros::transformPointCloud("/base_footprint", input, pc_footprint, *tf_listener);

    //source cloud
    pcl::PointCloud<pcl::PointXYZRGB> cloud_src;
    pcl::fromROSMsg(pc_footprint , cloud_src);
    //ROS_WARN("cloud_src size = %d  width = %d",cloud_src.size(),input.width); 

    ////////////////////////////////
    // Draw Face Boxes
    line_face.points.clear();
    line_face.header.frame_id = "base_footprint";
    line_face.ns = "line_face";
    line_face.action = visualization_msgs::Marker::ADD;
    line_face.id = 1;
    line_face.type = visualization_msgs::Marker::LINE_LIST;
    line_face.scale.x = 0.01;
    line_face.color.r = 1.0;
    line_face.color.g = 0;
    line_face.color.b = 1.0;
    line_face.color.a = 1.0;

    pos_face.points.clear();
    pos_face.header.frame_id = "base_footprint";
    pos_face.ns = "pos_face";
    pos_face.action = visualization_msgs::Marker::ADD;
    pos_face.id = 1;
    pos_face.type = visualization_msgs::Marker::CUBE_LIST;
    pos_face.scale.x = 0.5;
    pos_face.scale.y = 0.5;
    pos_face.scale.z = 0.001;
    pos_face.color.r = 1.0;
    pos_face.color.g = 0;
    pos_face.color.b = 1.0;
    pos_face.color.a = 1.0;

    geometry_msgs::Point p;
    int nFaceIndex = 1;
    std::vector<cv::Rect>::const_iterator i;
    for (face_iter = faces.begin(); face_iter != faces.end(); ++face_iter) 
    {
        int rgb_face_x = face_iter->x  + face_iter->width/2;
        int rgb_face_y = face_iter->y + face_iter->height/2;
        int index_pc = rgb_face_y*input.width + rgb_face_x;
        float face_x = cloud_src.points[index_pc].x;
        float face_y = cloud_src.points[index_pc].y;
        float face_z = cloud_src.points[index_pc].z;
       
        p.x = 0.2; p.y = 0; p.z = 1.37; line_face.points.push_back(p);
        //p.x = -0.1; p.y = 0; p.z = 1.25; line_face.points.push_back(p);
        p.x = face_x; p.y = face_y; p.z = face_z; line_face.points.push_back(p);
        p.z = 0;pos_face.points.push_back(p);

        std::ostringstream stringStream;
        stringStream << "Face_" << nFaceIndex;
        std::string face_id = stringStream.str();
        DrawTextMarker(face_id,nFaceIndex,0.1,face_x,face_y,face_z+0.2,0,1.0,0);
        nFaceIndex ++;

        ROS_WARN("face (%d,%d) - (%.2f %.2f %.2f)",rgb_face_x,rgb_face_y,face_x,face_y,face_z); 
    }
    marker_pub.publish(line_face);
    marker_pub.publish(pos_face);

    for(int y=0; y< 300; y++)
    {
        for(int x=0; x< 200; x++)
        {
            int index_pc = y*input.width + x;
            cloud_src.points[index_pc].r = 1.0f;
            cloud_src.points[index_pc].g = 0.0f;
            cloud_src.points[index_pc].b = 1.0f;
        }
    }
    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(cloud_src, output);
    output.header.frame_id = pc_footprint.header.frame_id;
    pc_pub.publish(output);

}

void PoseDiffCallback(const geometry_msgs::Pose2D::ConstPtr& msg)
{
    pose_diff.x = msg->x;
    pose_diff.y = msg->y;
    pose_diff.theta = msg->theta;
}

void KeywordCB(const std_msgs::String::ConstPtr & msg)
{
    ROS_WARN("[PersonReco KeywordCB] - %s",msg->data.c_str());
    string strListen = msg->data;

    if(nState == STATE_READY)
    {
        //从听到的句子里Robot/Robert
        int nFindRobot = strListen.find("Robot");
        int nFindRobert = strListen.find("Robert");
        if( nFindRobot>=0 || nFindRobert>=0 )
        {
            Speak("hello, Please look at me. I am going to memory you.");
            //识别完毕,关闭语音识别
            srvIAT.request.active = false;
            clientIAT.call(srvIAT);
            usleep(3*1000*1000);
            nState = STATE_OPERATOR;
        }
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "person_recognition");
    ros::NodeHandle nh_param("~");
    //nh_param.param<std::string>("rgb_topic", rgb_topic, "/camera/image_raw");
    nh_param.param<std::string>("rgb_topic", rgb_topic, "/kinect2/hd/image_color");
    nh_param.param<std::string>("topic", pc_topic, "/kinect2/hd/points");
    nh_param.param<std::string>("face_cascade_name", face_cascade_name, "haarcascade_frontalface_alt.xml");
 
    ROS_INFO("person_recognition");

    bool res = face_cascade.load(face_cascade_name);
	if (res == false)
	{
		ROS_ERROR("fail to load haarcascade_frontalface_alt.xml");
        return 0;
	}

    tf_listener = new tf::TransformListener(); 
    ros::NodeHandle n;
    ros::Subscriber rgb_sub = n.subscribe(rgb_topic, 1 , callbackRGB);
    ros::Subscriber pc_sub = n.subscribe(pc_topic, 1 , callbackPointCloud);
    image_pub = n.advertise<sensor_msgs::Image>("/face/image", 2);
    marker_pub = n.advertise<visualization_msgs::Marker>("face_marker", 2);
    pc_pub = n.advertise<sensor_msgs::PointCloud2>("face_pointcloud",1);
    ros::Subscriber sub_sr = n.subscribe("/xfyun/iat", 10, KeywordCB);
    spk_pub = n.advertise<sound_play::SoundRequest>("/robotsound", 20);
    clientIAT = n.serviceClient<xfyun_waterplus::IATSwitch>("xfyun_waterplus/IATSwitch");
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    ctrl_pub = n.advertise<std_msgs::String>("/wpb_home/ctrl", 30);
    ros::Subscriber pose_diff_sub = n.subscribe("/wpb_home/pose_diff", 1, PoseDiffCallback);

    ros::Rate loop_rate(30);
    while( ros::ok())
    {
        if(nState == STATE_COUNTDOWN)
        {
            //ROS_INFO("[Countdown] - %d",nCountDown);
            if(nCountDown > 0)
            {
                if(nCountDown%30 == 0)
                {
                    std::ostringstream stringStream;
                    stringStream << nCountDown/30;
                    std::string retStr = stringStream.str();
                    Speak(retStr);
                    ctrl_msg.data = "pose_diff reset";
                    ctrl_pub.publish(ctrl_msg);
                    ros::spinOnce();
                    ROS_INFO("[Countdown] - %s",retStr.c_str());
                    sleep(1);
                }
                nCountDown --;
            }
            else
            {
                Speak("OK,I am coming...");
                nState = STATE_TURN;
            }
        }
        if(nState == STATE_TURN)
        {
            ROS_INFO("[Turn] count=%d  z= %.2f",nTurnCount, pose_diff.theta);
            geometry_msgs::Twist vel_cmd;
            vel_cmd.linear.x = 0;
            vel_cmd.linear.y = 0;
            vel_cmd.linear.z = 0;
            vel_cmd.angular.x = 0;
            vel_cmd.angular.y = 0;
            vel_cmd.angular.z = 0.2;    //旋转速度,如果转身不理想,可以修改这个值
            nTurnCount --;
            if(pose_diff.theta >= 3.13)
            {
                vel_cmd.angular.z = 0;
                nCountDown ++;
                if(nCountDown > 100)
                {
                    nState = STATE_CROWD;
                }
            }
            vel_pub.publish(vel_cmd);
        }
        if(nState == STATE_CROWD)
        {
            geometry_msgs::Twist vel_cmd;
            vel_cmd.linear.x = 0;
            vel_cmd.linear.y = 0;
            vel_cmd.linear.z = 0;
            vel_cmd.angular.x = 0;
            vel_cmd.angular.y = 0;
            vel_cmd.angular.z = 0;
            vel_pub.publish(vel_cmd);
        }
        ros::spinOnce();
        loop_rate.sleep();
    }

    delete tf_listener; 

    return 0;
}

<|start_filename|>robot/src/drivers/waterplus_map_tools/src/wp_edit_node.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* @author <NAME>                                             */

#include <tinyxml.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <waterplus_map_tools/Waypoint.h>
#include <waterplus_map_tools/GetNumOfWaypoints.h>
#include <waterplus_map_tools/GetWaypointByIndex.h>
#include <waterplus_map_tools/GetWaypointByName.h>
#include <waterplus_map_tools/SaveWaypoints.h>
#include <interactive_markers/interactive_marker_server.h>
#include <interactive_markers/menu_handler.h>
#include <string>

using namespace visualization_msgs;
using namespace interactive_markers;
using namespace std;

static std::vector <waterplus_map_tools::Waypoint> arWaypoint;
static std::vector <waterplus_map_tools::Waypoint> arCharger;
static ros::Publisher marker_pub;
static visualization_msgs::Marker text_marker;
static InteractiveMarkerServer* pWaypointServer = NULL;
static MenuHandler* pMenuWaypoint = NULL;
static MenuHandler* pMenuCharger = NULL;

bool bDeleteWaypoint = false;
std::string strDelWaypointName;
bool bDeleteCharger = false;
std::string strDelChargerName;

bool getNumOfWaypoints(waterplus_map_tools::GetNumOfWaypoints::Request &req, waterplus_map_tools::GetNumOfWaypoints::Response &res)
{
    res.num = arWaypoint.size();
    ROS_INFO("Get_num_wp: num_wp = %d", res.num);
    return true;
}

bool getWaypointByIndex(waterplus_map_tools::GetWaypointByIndex::Request &req, waterplus_map_tools::GetWaypointByIndex::Response &res)
{
    int nIndex = req.index;
    int nNumWP = arWaypoint.size();
    if(nIndex >= 0 && nIndex < nNumWP)
    {
        res.name = arWaypoint[nIndex].name;
        res.pose = arWaypoint[nIndex].pose;
        ROS_INFO("Get_wp_index: name = %s", arWaypoint[nIndex].name.c_str());
        return true;
    }
    else
    {
        ROS_INFO("Get_wp_index: failed! index = %d , num_wp = %d", nIndex , nNumWP);
        return false;
    }
}

bool getWaypointByName(waterplus_map_tools::GetWaypointByName::Request &req, waterplus_map_tools::GetWaypointByName::Response &res)
{
    std::string reqName = req.name;
    int nNumWP = arWaypoint.size();
    bool bResultGetWP = false;
    for(int i=0;i<nNumWP;i++)
    {
        std::size_t found = arWaypoint[i].name.find(reqName); 
        if(found != std::string::npos)
        {
            res.name = arWaypoint[i].name;
            res.pose = arWaypoint[i].pose;
            bResultGetWP = true;
            break;
        }
    }
    if(bResultGetWP == true)
    {
        ROS_INFO("Get_wp_name: name = %s", res.name.c_str());
        return true;
    }
    else
    {
        ROS_INFO("Get_wp_name: failed! There is no waypoint name %s", reqName.c_str());
        return false;
    }
}

bool getChargerByName(waterplus_map_tools::GetWaypointByName::Request &req, waterplus_map_tools::GetWaypointByName::Response &res)
{
    std::string reqName = req.name;
    int nNumCh = arCharger.size();
    bool bResultGetCh = false;
    for(int i=0;i<nNumCh;i++)
    {
        std::size_t found = arCharger[i].name.find(reqName); 
        if(found != std::string::npos)
        {
            res.name = arCharger[i].name;
            res.pose = arCharger[i].pose;
            bResultGetCh = true;
            break;
        }
    }
    if(bResultGetCh == true)
    {
        ROS_INFO("Get_charger_name: name = %s", res.name.c_str());
        return true;
    }
    else
    {
        ROS_INFO("Get_charger_name: failed! There is no charger name %s", reqName.c_str());
        return false;
    }
}

bool SaveWaypointsToFile(std::string inFilename);
bool saveWaypoints(waterplus_map_tools::SaveWaypoints::Request &req, waterplus_map_tools::SaveWaypoints::Response &res)
{
    return SaveWaypointsToFile(req.filename);
}

std::string Flt2Str(float inVal)
{
    std::ostringstream stringStream;
    stringStream << inVal;
    std::string retStr = stringStream.str();
    return retStr;
}

std::string Int2Str(int inVal)
{
    std::ostringstream stringStream;
    stringStream << inVal;
    std::string retStr = stringStream.str();
    return retStr;
}

bool SaveWaypointsToFile(std::string inFilename)
{
    TiXmlDocument *docSave = new TiXmlDocument();
    TiXmlElement *RootElement = new TiXmlElement("Waterplus");
    docSave->LinkEndChild(RootElement);

    int nNumWP = arWaypoint.size();
    for(int i=0;i<nNumWP;i++)
    {
        TiXmlElement *WaypointElement = new TiXmlElement("Waypoint");
        WaypointElement->InsertEndChild(TiXmlElement("Name"))->InsertEndChild(TiXmlText(arWaypoint[i].name));
        WaypointElement->InsertEndChild(TiXmlElement("Pos_x"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.position.x)));
        WaypointElement->InsertEndChild(TiXmlElement("Pos_y"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.position.y)));
        WaypointElement->InsertEndChild(TiXmlElement("Pos_z"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.position.z)));
        WaypointElement->InsertEndChild(TiXmlElement("Ori_x"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.x)));
        WaypointElement->InsertEndChild(TiXmlElement("Ori_y"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.y)));
        WaypointElement->InsertEndChild(TiXmlElement("Ori_z"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.z)));
        WaypointElement->InsertEndChild(TiXmlElement("Ori_w"))->InsertEndChild(TiXmlText(Flt2Str(arWaypoint[i].pose.orientation.w)));
        RootElement->InsertEndChild(*WaypointElement);  
    }

    int nNumCharger = arCharger.size();
    for(int i=0;i<nNumCharger;i++)
    {
        TiXmlElement *ChargerElement = new TiXmlElement("Charger");
        ChargerElement->InsertEndChild(TiXmlElement("Name"))->InsertEndChild(TiXmlText(arCharger[i].name));
        ChargerElement->InsertEndChild(TiXmlElement("Pos_x"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.position.x)));
        ChargerElement->InsertEndChild(TiXmlElement("Pos_y"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.position.y)));
        ChargerElement->InsertEndChild(TiXmlElement("Pos_z"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.position.z)));
        ChargerElement->InsertEndChild(TiXmlElement("Ori_x"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.x)));
        ChargerElement->InsertEndChild(TiXmlElement("Ori_y"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.y)));
        ChargerElement->InsertEndChild(TiXmlElement("Ori_z"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.z)));
        ChargerElement->InsertEndChild(TiXmlElement("Ori_w"))->InsertEndChild(TiXmlText(Flt2Str(arCharger[i].pose.orientation.w)));
        RootElement->InsertEndChild(*ChargerElement);  
    }

    bool res = docSave->SaveFile(inFilename);
    if(res == true)
        ROS_INFO("Saved waypoints to file! filename = %s", inFilename.c_str());
    else
        ROS_INFO("Failed to save waypoints... filename = %s", inFilename.c_str());

    return res;
}

bool LoadWaypointsFromFile(std::string inFilename)
{
    TiXmlDocument docLoad(inFilename);
    bool resLoad = docLoad.LoadFile();
    if(resLoad == false)
    {
        ROS_INFO("Failed to load waypoints... filename = %s", inFilename.c_str());
        return false;
    }

    waterplus_map_tools::Waypoint newWayPoint;
    TiXmlElement* RootElement = docLoad.RootElement();
    for(TiXmlNode* item = RootElement->FirstChild("Waypoint");item;item = item->NextSibling("Waypoint"))
    {
        TiXmlNode* child = item->FirstChild();
        const char* name = child->ToElement()->GetText();
        ROS_INFO("Load waypoint : %s", name);
        newWayPoint.name = std::string(name); 
        child = item->IterateChildren(child);
        const char* pos_x = child->ToElement()->GetText();
        newWayPoint.pose.position.x = std::atof(pos_x);
        child = item->IterateChildren(child);
        const char* pos_y = child->ToElement()->GetText();
        newWayPoint.pose.position.y = std::atof(pos_y);
        child = item->IterateChildren(child);
        const char* pos_z = child->ToElement()->GetText();
        newWayPoint.pose.position.z = std::atof(pos_z);
        child = item->IterateChildren(child);
        const char* ori_x = child->ToElement()->GetText();
        newWayPoint.pose.orientation.x = std::atof(ori_x);
        child = item->IterateChildren(child);
        const char* ori_y = child->ToElement()->GetText();
        newWayPoint.pose.orientation.y = std::atof(ori_y);
        child = item->IterateChildren(child);
        const char* ori_z = child->ToElement()->GetText();
        newWayPoint.pose.orientation.z = std::atof(ori_z);
        child = item->IterateChildren(child);
        const char* ori_w = child->ToElement()->GetText();
        newWayPoint.pose.orientation.w = std::atof(ori_w);
        arWaypoint.push_back(newWayPoint);
    }

    for(TiXmlNode* item = RootElement->FirstChild("Charger");item;item = item->NextSibling("Charger"))
    {
        TiXmlNode* child = item->FirstChild();
        const char* name = child->ToElement()->GetText();
        ROS_INFO("Load charger : %s", name);
        newWayPoint.name = std::string(name); 
        child = item->IterateChildren(child);
        const char* pos_x = child->ToElement()->GetText();
        newWayPoint.pose.position.x = std::atof(pos_x);
        child = item->IterateChildren(child);
        const char* pos_y = child->ToElement()->GetText();
        newWayPoint.pose.position.y = std::atof(pos_y);
        child = item->IterateChildren(child);
        const char* pos_z = child->ToElement()->GetText();
        newWayPoint.pose.position.z = std::atof(pos_z);
        child = item->IterateChildren(child);
        const char* ori_x = child->ToElement()->GetText();
        newWayPoint.pose.orientation.x = std::atof(ori_x);
        child = item->IterateChildren(child);
        const char* ori_y = child->ToElement()->GetText();
        newWayPoint.pose.orientation.y = std::atof(ori_y);
        child = item->IterateChildren(child);
        const char* ori_z = child->ToElement()->GetText();
        newWayPoint.pose.orientation.z = std::atof(ori_z);
        child = item->IterateChildren(child);
        const char* ori_w = child->ToElement()->GetText();
        newWayPoint.pose.orientation.w = std::atof(ori_w);
        arCharger.push_back(newWayPoint);
    }

    return true;
}

void DrawTextMarker(ros::Publisher* inPub, std::string inText, int inID, float inScale, float inX, float inY, float inZ, float inR, float inG, float inB)
{
    text_marker.header.frame_id = "map";
    text_marker.ns = "text";
    text_marker.action = visualization_msgs::Marker::ADD;
    text_marker.id = inID;
    text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    text_marker.scale.z = inScale;
    text_marker.color.r = inR;
    text_marker.color.g = inG;
    text_marker.color.b = inB;
    text_marker.color.a = 1.0;

    text_marker.pose.position.x = inX;
    text_marker.pose.position.y = inY;
    text_marker.pose.position.z = inZ;
    
    text_marker.pose.orientation=tf::createQuaternionMsgFromYaw(1.0);

    text_marker.text = inText;

    inPub->publish(text_marker);
}

void RemoveTextMarker()
{
    text_marker.action = 3;
    marker_pub.publish(text_marker);
}

void PublishTextMarker()
{
    int nNumWP = arWaypoint.size();
    for(int i=0; i<nNumWP ; i++ )
    {
        float wp_x = arWaypoint[i].pose.position.x;
        float wp_y = arWaypoint[i].pose.position.y;

        std::string wp_name = arWaypoint[i].name;
        DrawTextMarker(&marker_pub,wp_name,i,0.2,wp_x,wp_y,0.55,1.0,1.0,0.0);
        ros::spinOnce();
    }

    int nNumCh = arCharger.size();
    for(int i=0; i<nNumCh ; i++ )
    {
      
        float ch_x = arCharger[i].pose.position.x;
        float ch_y = arCharger[i].pose.position.y;

        std::string wp_name = arCharger[i].name;
        DrawTextMarker(&marker_pub, wp_name,nNumWP+i,0.2,ch_x,ch_y,0.35,1.0,1.0,0.0);
        ros::spinOnce();
    }
}

// 移动航点的回调函数
void processWaypointFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
    ROS_WARN("[%s] p(%.2f,%.2f,%.2f) r(%.2f,%.2f,%.2f,%.2f)",feedback->marker_name.c_str(),
    feedback->pose.position.x,feedback->pose.position.y,feedback->pose.position.z,
    feedback->pose.orientation.x,feedback->pose.orientation.y,feedback->pose.orientation.z,feedback->pose.orientation.w);

    int nNumWP = arWaypoint.size();
    for(int i=0; i<nNumWP ; i++ )
    {
        if(feedback->marker_name == arWaypoint[i].name)
        {
            arWaypoint[i].pose = feedback->pose;
        }
    }

    std::ostringstream s;
    std::ostringstream mouse_point_ss;
    if( feedback->mouse_point_valid )
    {
        switch(feedback->event_type)
        {
            case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
                //ROS_INFO("BUTTON_CLICK");
                break;
            case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
                //ROS_INFO("MOUSE_DOWN");
                break;
            case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
                //ROS_INFO("MOUSE_UP");
                break;
            case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
                //ROS_INFO_STREAM( s.str() << ": menu item " << feedback->menu_entry_id << " clicked" << mouse_point_ss.str() << "." );
                break;
        }
    }
}

// 移动充电桩的回调函数
void processChargerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
    ROS_WARN("[%s] p(%.2f,%.2f,%.2f) r(%.2f,%.2f,%.2f,%.2f)",feedback->marker_name.c_str(),
    feedback->pose.position.x,feedback->pose.position.y,feedback->pose.position.z,
    feedback->pose.orientation.x,feedback->pose.orientation.y,feedback->pose.orientation.z,feedback->pose.orientation.w);

    int nNumChargers = arCharger.size();
    for(int i=0; i<nNumChargers ; i++ )
    {
        if(feedback->marker_name == arCharger[i].name)
        {
            arCharger[i].pose = feedback->pose;
        }
    }

    std::ostringstream s;
    std::ostringstream mouse_point_ss;
    if( feedback->mouse_point_valid )
    {
        switch(feedback->event_type)
        {
            case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
                //ROS_INFO("BUTTON_CLICK");
                break;
            case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
                //ROS_INFO("MOUSE_DOWN");
                break;
            case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
                //ROS_INFO("MOUSE_UP");
                break;
            case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
                //ROS_INFO_STREAM( s.str() << ": menu item " << feedback->menu_entry_id << " clicked" << mouse_point_ss.str() << "." );
                break;
        }
    }
}

// 向服务器添加新的航点操作标记
void NewWaypointInterMarker(InteractiveMarkerServer* inServer,string inName, geometry_msgs::Pose InPose)
{
    visualization_msgs::InteractiveMarker wp_itr_marker;
    visualization_msgs::InteractiveMarkerControl wp_dis_ctrl;
    visualization_msgs::Marker wp_dis_marker;
    visualization_msgs::InteractiveMarkerControl move_control;
    wp_itr_marker.header.stamp=ros::Time::now();
    wp_itr_marker.name = inName;
    wp_itr_marker.description = inName;
    wp_itr_marker.pose = InPose;

    // 显示外形
    wp_itr_marker.header.frame_id = "map";
    wp_itr_marker.header.stamp=ros::Time::now();

    wp_dis_marker.ns = "marker_waypoints";
    wp_dis_marker.action = visualization_msgs::Marker::ADD;
    wp_dis_marker.type = visualization_msgs::Marker::MESH_RESOURCE;
    wp_dis_marker.mesh_resource = "package://waterplus_map_tools/meshes/waypoint.dae";
    wp_dis_marker.scale.x = 1;
    wp_dis_marker.scale.y = 1;
    wp_dis_marker.scale.z = 1;
    wp_dis_marker.color.r = 1.0;
    wp_dis_marker.color.g = 0.0;
    wp_dis_marker.color.b = 1.0;
    wp_dis_marker.color.a = 1.0;

    wp_dis_ctrl.markers.push_back( wp_dis_marker );
    wp_dis_ctrl.always_visible = true;
    wp_itr_marker.controls.push_back( wp_dis_ctrl );

    // 操作设置
    move_control.name = "move_x";
    move_control.orientation.w = 1.0;
	move_control.orientation.x = 1.0;
	move_control.orientation.y = 0.0;
	move_control.orientation.z = 0.0;
    move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
    wp_itr_marker.controls.push_back(move_control);
    move_control.name = "move_z";
	move_control.orientation.x = 0.0;
	move_control.orientation.z = 1.0;
    wp_itr_marker.controls.push_back(move_control);
    move_control.name = "rotate_z";
	move_control.orientation.x = 0.0;
	move_control.orientation.y = 1.0;
	move_control.orientation.z = 0.0;
    move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
    wp_itr_marker.controls.push_back(move_control);

    // 加入菜单
    visualization_msgs::Marker menu_marker;
    menu_marker.type = visualization_msgs::Marker::CUBE;
    menu_marker.scale.x = 0.5;
    menu_marker.scale.y = 0.5;
    menu_marker.scale.z = 0.5;
    menu_marker.color.r = 0.9;
    menu_marker.color.g = 0.9;
    menu_marker.color.b = 0.9;
    menu_marker.color.a = 0.0;  //全透明
    InteractiveMarkerControl menu_control;
    menu_control.interaction_mode = InteractiveMarkerControl::BUTTON;
    menu_control.always_visible = true;
    menu_control.markers.push_back( menu_marker );
    wp_itr_marker.controls.push_back( menu_control );

    inServer->insert(wp_itr_marker, &processWaypointFeedback);
}

// 向服务器添加新的充电桩操作标记
void NewChargerInterMarker(InteractiveMarkerServer* inServer,string inName, geometry_msgs::Pose InPose)
{
    visualization_msgs::InteractiveMarker wp_itr_marker;
    visualization_msgs::InteractiveMarkerControl wp_dis_ctrl;
    visualization_msgs::Marker wp_dis_marker;
    visualization_msgs::InteractiveMarkerControl move_control;
    wp_itr_marker.header.stamp=ros::Time::now();
    wp_itr_marker.name = inName;
    wp_itr_marker.description = inName;
    wp_itr_marker.pose = InPose;

    // 显示外形
    wp_itr_marker.header.frame_id = "map";
    wp_itr_marker.header.stamp=ros::Time::now();

    wp_dis_marker.ns = "marker_chargers";
    wp_dis_marker.action = visualization_msgs::Marker::ADD;
    wp_dis_marker.type = visualization_msgs::Marker::MESH_RESOURCE;
    wp_dis_marker.mesh_resource = "package://waterplus_map_tools/meshes/charger.dae";
    wp_dis_marker.scale.x = 1;
    wp_dis_marker.scale.y = 1;
    wp_dis_marker.scale.z = 1;
    wp_dis_marker.color.r = 0.0;
    wp_dis_marker.color.g = 0.0;
    wp_dis_marker.color.b = 1.0;
    wp_dis_marker.color.a = 1.0;

    wp_dis_ctrl.markers.push_back( wp_dis_marker );
    wp_dis_ctrl.always_visible = true;
    wp_itr_marker.controls.push_back( wp_dis_ctrl );

    // 操作设置
    move_control.name = "move_x";
    move_control.orientation.w = 1.0;
	move_control.orientation.x = 1.0;
	move_control.orientation.y = 0.0;
	move_control.orientation.z = 0.0;
    move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
    wp_itr_marker.controls.push_back(move_control);
    move_control.name = "move_z";
	move_control.orientation.x = 0.0;
	move_control.orientation.z = 1.0;
    wp_itr_marker.controls.push_back(move_control);
    move_control.name = "rotate_z";
	move_control.orientation.x = 0.0;
	move_control.orientation.y = 1.0;
	move_control.orientation.z = 0.0;
    move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
    wp_itr_marker.controls.push_back(move_control);

    // 加入菜单
    visualization_msgs::Marker menu_marker;
    menu_marker.type = visualization_msgs::Marker::CUBE;
    menu_marker.scale.x = 0.5;
    menu_marker.scale.y = 0.5;
    menu_marker.scale.z = 0.5;
    menu_marker.color.r = 0.9;
    menu_marker.color.g = 0.9;
    menu_marker.color.b = 0.9;
    menu_marker.color.a = 0.0;  //全透明
    InteractiveMarkerControl menu_control;
    menu_control.interaction_mode = InteractiveMarkerControl::BUTTON;
    menu_control.always_visible = true;
    menu_control.markers.push_back( menu_marker );
    wp_itr_marker.controls.push_back( menu_control );

    inServer->insert(wp_itr_marker, &processChargerFeedback);
}

// 菜单删除航点的回调函数
void DeleteWaypointCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
  strDelWaypointName = feedback->marker_name;
  bDeleteWaypoint = true;
  ROS_WARN("Menu - Delete waypoint %s",strDelWaypointName.c_str());
}

// 菜单删除充电桩的回调函数
void DeleteChargerCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
  strDelChargerName = feedback->marker_name;
  bDeleteCharger = true;
  ROS_WARN("Menu - Delete charger %s",strDelChargerName.c_str());
}

// 添加新航点回调函数
void AddWayPointCallback(const waterplus_map_tools::Waypoint::ConstPtr& wp)
{
    ROS_INFO("Add_waypoint: %s (%.2f %.2f) (%.2f %.2f %.2f %.2f) ",wp->name.c_str(), wp->pose.position.x, wp->pose.position.y, wp->pose.orientation.x, wp->pose.orientation.y, wp->pose.orientation.z, wp->pose.orientation.w);
    waterplus_map_tools::Waypoint newWayPoint;
    newWayPoint = *wp;
    int nWPNum = arWaypoint.size();
    for(int i= 0;i<nWPNum;i++)
    {
        if(newWayPoint.name == arWaypoint[i].name)
        {
            newWayPoint.name = newWayPoint.name + "_1";
        }
    }
    arWaypoint.push_back(newWayPoint);
    if(pWaypointServer != NULL)
    {
        NewWaypointInterMarker( pWaypointServer, newWayPoint.name, newWayPoint.pose );
        pMenuWaypoint->apply( *pWaypointServer, newWayPoint.name );
        //通知client（RVIZ）更新显示
        pWaypointServer->applyChanges();
    }
}

// 添加新充电桩回调函数
void AddChargerCallback(const waterplus_map_tools::Waypoint::ConstPtr& wp)
{
    ROS_INFO("Add_charger: %s (%.2f %.2f) (%.2f %.2f %.2f %.2f) ",wp->name.c_str(), wp->pose.position.x, wp->pose.position.y, wp->pose.orientation.x, wp->pose.orientation.y, wp->pose.orientation.z, wp->pose.orientation.w);
    waterplus_map_tools::Waypoint newCharger;
    newCharger = *wp;
    int nChargerNum = arCharger.size();
    for(int i= 0;i<nChargerNum;i++)
    {
        if(newCharger.name == arCharger[i].name)
        {
            newCharger.name = newCharger.name + "_1";
        }
    }
    arCharger.push_back(newCharger);
    if(pWaypointServer != NULL)
    {
        NewChargerInterMarker( pWaypointServer, newCharger.name, newCharger.pose );
        pMenuCharger->apply( *pWaypointServer, newCharger.name );
        //通知client（RVIZ）更新显示
        pWaypointServer->applyChanges();
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "wp_edit_node");

    std::string strLoadFile;
    char const* home = getenv("HOME");
    strLoadFile = home;
    strLoadFile += "/waypoints.xml";
    
    ros::NodeHandle n_param("~");
    std::string strParamFile;
    n_param.param<std::string>("load", strParamFile, "");
    if(strParamFile.length() > 0)
    {
        strLoadFile = strParamFile;
    }

    if(strLoadFile.length() > 0)
    {
        ROS_INFO("Load waypoints from file : %s",strLoadFile.c_str());
        LoadWaypointsFromFile(strLoadFile);
    }
    else
    {
        ROS_WARN("strLoadFile is empty. Failed to load waypoints!");
    }

    ros::NodeHandle nh;

    //创建服务
    interactive_markers::InteractiveMarkerServer wp_server("waypoints_move");
    pWaypointServer = &wp_server;
    
    marker_pub = nh.advertise<visualization_msgs::Marker>("text_marker", 100);
    ros::Subscriber add_waypoint_sub = nh.subscribe("/waterplus/add_waypoint",10,&AddWayPointCallback);
    ros::Subscriber add_charger_sub = nh.subscribe("/waterplus/add_charger",10,&AddChargerCallback);
    ros::ServiceServer srvGetNum = nh.advertiseService("/waterplus/get_num_waypoint", getNumOfWaypoints);
    ros::ServiceServer srvGetWPIndex = nh.advertiseService("/waterplus/get_waypoint_index", getWaypointByIndex);
    ros::ServiceServer srvGetWPName = nh.advertiseService("/waterplus/get_waypoint_name", getWaypointByName);
    ros::ServiceServer srvSaveWP = nh.advertiseService("/waterplus/save_waypoints", saveWaypoints);
    ros::ServiceServer srvGetChargerName = nh.advertiseService("/waterplus/get_charger_name", getChargerByName);

    //将互动标记放到标记集合里，同时指定Feedback回调函数
    int nWPNum = arWaypoint.size();
    ROS_INFO("Num of waypoints = %d",nWPNum);
    for(int i=0; i< nWPNum; i++)
    {
        NewWaypointInterMarker( &wp_server, arWaypoint[i].name, arWaypoint[i].pose );
    }
    MenuHandler menu_waypoint;
    menu_waypoint.insert( "Delete", &DeleteWaypointCallback);
    pMenuWaypoint = &menu_waypoint;
    for(int i=0; i< nWPNum; i++)
    {
        menu_waypoint.apply( wp_server, arWaypoint[i].name );
    }
    // 充电桩标记初始化
    int nChargerNum = arCharger.size();
    ROS_INFO("Num of chargers = %d",nChargerNum);
    for(int i=0; i< nChargerNum; i++)
    {
        NewChargerInterMarker( &wp_server, arCharger[i].name, arCharger[i].pose );
    }
    MenuHandler menu_charger;
    menu_charger.insert( "Delete", &DeleteChargerCallback);
    pMenuCharger = &menu_charger;
    for(int i=0; i< nChargerNum; i++)
    {
        menu_charger.apply( wp_server, arCharger[i].name );
    }

    //通知client（RVIZ）更新显示
    wp_server.applyChanges();

    ros::Rate r(10);

    while(ros::ok())
    {
        if(bDeleteWaypoint == true)
        {
            bDeleteWaypoint = false;
            wp_server.erase(strDelWaypointName);
            wp_server.applyChanges();
            for(vector<waterplus_map_tools::Waypoint>::iterator iter=arWaypoint.begin(); iter!=arWaypoint.end(); )
            {
                if( (*iter).name == strDelWaypointName)
                    iter = arWaypoint.erase(iter);
                else
                    iter ++ ;
            }
            ROS_WARN("%s waypoint deleted!",strDelWaypointName.c_str());
            RemoveTextMarker();
        }
        if(bDeleteCharger == true)
        {
            bDeleteCharger = false;
            wp_server.erase(strDelChargerName);
            wp_server.applyChanges();
            for(vector<waterplus_map_tools::Waypoint>::iterator iter=arCharger.begin(); iter!=arCharger.end(); )
            {
                if( (*iter).name == strDelChargerName)
                    iter = arCharger.erase(iter);
                else
                    iter ++ ;
            }
            ROS_WARN("%s charger deleted!",strDelChargerName.c_str());
            RemoveTextMarker();
        }
        PublishTextMarker();
        ros::spinOnce();
        r.sleep();
    }

    return 0;
}

<|start_filename|>robot/src/fight_with_hair_ui/src/build_map.cpp<|end_filename|>
#include "../include/fight_with_hair_ui/build_map.h"
#include "ui_build_map.h"
#include "../include/fight_with_hair_ui/mainframe.h"
#include <QMessageBox>
#include <QKeyEvent>
#include <QString>
#include <stdio.h>
#include <string>
#include <QAbstractItemView>
#include <QCloseEvent>
build_map::build_map(bool isSim,QWidget *parent) :
  QWidget(parent),
  ui(new Ui::build_map)
{
  ui->setupUi(this);
//  this->setFixedSize(this->geometry().width(),this->geometry().height());

//  QString mapStr = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch slam gmapping.launch'&";
//  system(mapStr.toLatin1().data());


  if (isSim){
    QString mapStr = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch slam gmapping_simulate.launch'&";
    system(mapStr.toLatin1().data());
  }else{
    QString mapStr = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch slam gmapping.launch'&";
    system(mapStr.toLatin1().data());
  }

  QString moveStr = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch user_listen keyboard_user_listen_for_map.launch'&";
  system(moveStr.toLatin1().data());

  ui->tableWidget->setEditTriggers(QAbstractItemView::NoEditTriggers);
  ui->tableWidget->horizontalHeader()->setStretchLastSection(true);
  render_panel_ = new rviz::RenderPanel;
  ui->mapdisplay_layout->addWidget(render_panel_);
  manager_ = new rviz::VisualizationManager(render_panel_);
  render_panel_->initialize(manager_->getSceneManager(), manager_);
  manager_->initialize();
  manager_->startUpdate();
}

build_map::~build_map()
{
  delete ui;
}


void build_map::ros_init(int argc,char **argv)
{
  ros::init(argc,argv,"build_map",ros::init_options::AnonymousName);
}

void build_map::on_logout_clicked()
{
  QMessageBox::StandardButton choice=QMessageBox::question(this,"确认","确定要退出吗？",QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes);
  if(choice==QMessageBox::Yes){
    this->close();
    QString string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /arm_move;"
                     "rosnode kill /basic_avoidance;"
                     "rosnode kill /basic_move;"
                     "rosnode kill /my_joy;"
                     "rosnode kill /keyboard_listen;"
                     "rosnode kill /user_listen;"
                     "rosnode kill /slam_gmapping;"
                     "rosnode kill /slam_lidar_filter;"
                     "rosnode kill /wpb_home_core;"
                     "rosnode kill /map_saver;'&";
   // system(string.toLatin1().data());
    string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /joint_state_publisher;rosnode kill /robot_state_publisher;'&";
    //system(string.toLatin1().data());
    QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
    system(killString.toLatin1().data());
    killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
    system(killString.toLatin1().data());
    QApplication::quit();
  }
}

void build_map::on_returnMain_clicked()
{
  //  mainframe *main=new mainframe;
  //  main->move(550,200);
  //  main->show();
  QString string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /arm_move;"
                   "rosnode kill /basic_avoidance;"
                   "rosnode kill /basic_move;"
                   "rosnode kill /my_joy;"
                   "rosnode kill /keyboard_listen;"
                   "rosnode kill /user_listen;"
                   "rosnode kill /slam_gmapping;"
                   "rosnode kill /slam_lidar_filter;"
                   "rosnode kill /wpb_home_core;"
                   "rosnode kill /map_saver;'&";
  if(!hasSaved){
    QMessageBox::StandardButton choice=QMessageBox::question(this,"Warning","地图尚未保存，确定退出建图界面？",QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes);
    if(choice==QMessageBox::Yes){
      //system(string.toLatin1().data());
      QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
      system(killString.toLatin1().data());
      killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
      system(killString.toLatin1().data());
      this->close();
    }
    return ;
  }
  //system(string.toLatin1().data());
  QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
  system(killString.toLatin1().data());
  killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
  system(killString.toLatin1().data());
  this->close();
}

void build_map::keyPressEvent(QKeyEvent *event){
  if(event->key() == Qt::Key_Escape){
    on_logout_clicked();
  }else if(event->key()==Qt::Key_W){
    ui->forward->setStyleSheet("color:red;");
    emit movemsg(0.3,0,0);
  }else if(event->key()==Qt::Key_A){
    ui->left->setStyleSheet("color:red;");
    emit movemsg(0,0,0.6);
  }else if(event->key()==Qt::Key_X){
    ui->backward->setStyleSheet("color:red;");
    emit movemsg(-0.3,0,0);
  }else if(event->key()==Qt::Key_D){
    ui->right->setStyleSheet("color:red;");
    emit movemsg(0,0,-0.6);
  }else if(event->key()==Qt::Key_S){
    ui->stop->setStyleSheet("color:red;");
    emit movemsg(0,0,0);
  }else if(event->key()==Qt::Key_Q){
    ui->left_forward->setStyleSheet("color:red;");
    emit movemsg(0.3,0,0.3);
  }else if(event->key()==Qt::Key_E){
    ui->right_forward->setStyleSheet("color:red;");
    emit movemsg(0.3,0,-0.3);
  }else if(event->key()==Qt::Key_Z){
    ui->left_back->setStyleSheet("color:red;");
    emit movemsg(-0.3,0,0.3);
  }else if(event->key()==Qt::Key_C){
    ui->right_back->setStyleSheet("color:red;");
    emit movemsg(-0.3,0,-0.3);
  }
}
void build_map::keyReleaseEvent(QKeyEvent *event) {
  if(event->key()==Qt::Key_W){
    ui->forward->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_A){
    ui->left->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_X){
    ui->backward->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_D){
    ui->right->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_S){
    ui->stop->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_Q){
    ui->left_forward->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_E){
    ui->right_forward->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_Z){
    ui->left_back->setStyleSheet("color:black;");
  }else if(event->key()==Qt::Key_C){
    ui->right_back->setStyleSheet("color:black;");
  }
}

void build_map::on_add_map_button_clicked()
{
  static int row=0;
  std::string str=std::to_string(row);
  const char *p = str.data();
  //    itoa(row,p,10);
  QString mapName=ui->input->text();
  ui->tableWidget->insertRow(ui->tableWidget->rowCount());
  ui->tableWidget->setItem(row, 0, new QTableWidgetItem(QString(p)));
  ui->tableWidget->setItem(row, 1, new QTableWidgetItem(mapName));
  row++;
  QString s1="gnome-terminal -x bash -c 'source ~/.bashrc;rosrun map_server map_saver -f ~/fight_with_hair/src/navi/maps/"+mapName+"'&";
  system(s1.toLatin1().data());
  QMessageBox::information(this, tr("提示"), tr("保存成功!"));
  hasSaved = true;
}

void build_map::on_map_show_btn_clicked()
{

  manager_->removeAllDisplays();

  rviz::Display *map_=manager_->createDisplay("rviz/Map","fwh_ui map",true);
  ROS_ASSERT(map_!=NULL);
  map_->subProp("Topic")->setValue("/map");

  rviz::Display *robot_=manager_->createDisplay("rviz/RobotModel","fwh_ui robot",true);
  ROS_ASSERT(robot_!=NULL);
  robot_->subProp("Robot Description")->setValue("robot_description");

  rviz::Display *laser_=manager_->createDisplay("rviz/LaserScan","fwh_ui laser",true);
  ROS_ASSERT(laser_!=NULL);
  laser_->subProp("Topic")->setValue("/scan");
  laser_->subProp("Size (m)")->setValue("0.1");
  manager_->startUpdate();
}

void build_map::on_forward_clicked()
{
  emit movemsg(0.3,0,0);
}

void build_map::on_right_forward_clicked()
{
  emit movemsg(0.3,0,-0.3);
}

void build_map::on_right_clicked()
{
  emit movemsg(0,0,-0.6);
}

void build_map::on_right_back_clicked()
{
  emit movemsg(0.3,0,-0.3);
}

void build_map::on_backward_clicked()
{
  emit movemsg(-0.3,0,0);
}

void build_map::on_left_back_clicked()
{
  emit movemsg(-0.3,0,0.3);
}

void build_map::on_left_clicked()
{
  emit movemsg(0,0,0.6);
}

void build_map::on_left_forward_clicked()
{
  emit movemsg(0.3,0,0.3);
}

void build_map::on_stop_clicked()
{
  emit movemsg(0,0,0);
}

void build_map::closeEvent(QCloseEvent *event){
  main->show();
  emit build_map_finish();
}

void build_map::setMain(fight_with_hair_ui::MainWindow *m){
  main=m;
}

void build_map::setEnvironment(bool s)
{
  isSim = s;
}


<|start_filename|>robot/src/fight_with_hair_ui/src/myviz.cpp<|end_filename|>
#include <QColor>
#include <QSlider>
#include <QLabel>
#include <QGridLayout>
#include <QVBoxLayout>

#include "rviz/visualization_manager.h"
#include "rviz/render_panel.h"
#include "rviz/display.h"

#include "../include/fight_with_hair_ui/myviz.h"
// add by xq
// BEGIN_TUTORIAL
// Constructor for MyViz.  This does most of the work of the class.
MyViz::MyViz( QWidget* parent )
  : QWidget( parent )
{
  // Construct and lay out labels and slider controls.
  QLabel* thickness_label = new QLabel( "Line Thickness" );
  QSlider* thickness_slider = new QSlider( Qt::Horizontal );
  thickness_slider->setMinimum( 1 );
  thickness_slider->setMaximum( 100 );
  QLabel* cell_size_label = new QLabel( "Cell Size" );
  QSlider* cell_size_slider = new QSlider( Qt::Horizontal );
  cell_size_slider->setMinimum( 1 );
  cell_size_slider->setMaximum( 100 );
  QGridLayout* controls_layout = new QGridLayout();
  controls_layout->addWidget( thickness_label, 0, 0 );
  controls_layout->addWidget( thickness_slider, 0, 1 );
  controls_layout->addWidget( cell_size_label, 1, 0 );
  controls_layout->addWidget( cell_size_slider, 1, 1 );

  // Construct and lay out render panel.
  render_panel_ = new rviz::RenderPanel();
  QVBoxLayout* main_layout = new QVBoxLayout;
  main_layout->addLayout( controls_layout );
  main_layout->addWidget( render_panel_ );

  // Set the top-level layout for this MyViz widget.
  setLayout( main_layout );

  // Make signal/slot connections.
  connect( thickness_slider, SIGNAL( valueChanged( int )), this, SLOT( setThickness( int )));
  connect( cell_size_slider, SIGNAL( valueChanged( int )), this, SLOT( setCellSize( int )));

  // Next we initialize the main RViz classes.
  //
  // The VisualizationManager is the container for Display objects,
  // holds the main Ogre scene, holds the ViewController, etc.  It is
  // very central and we will probably need one in every usage of
  // librviz.
  manager_ = new rviz::VisualizationManager( render_panel_ );
  render_panel_->initialize( manager_->getSceneManager(), manager_ );
  manager_->initialize();
  manager_->startUpdate();

  // Create a Grid display.
  grid_ = manager_->createDisplay( "rviz/Grid", "adjustable grid", true );
  ROS_ASSERT( grid_ != NULL );

  // Configure the GridDisplay the way we like it.
  grid_->subProp( "Line Style" )->setValue( "Billboards" );
  grid_->subProp( "Color" )->setValue( QColor( Qt::yellow ) );

  // Initialize the slider values.
  thickness_slider->setValue( 25 );
  cell_size_slider->setValue( 10 );
}

// Destructor.
MyViz::~MyViz()
{
  delete manager_;
}

// This function is a Qt slot connected to a QSlider's valueChanged()
// signal.  It sets the line thickness of the grid by changing the
// grid's "Line Width" property.
void MyViz::setThickness( int thickness_percent )
{
  if( grid_ != NULL )
  {
    grid_->subProp( "Line Style" )->subProp( "Line Width" )->setValue( thickness_percent / 100.0f );
  }
}

// This function is a Qt slot connected to a QSlider's valueChanged()
// signal.  It sets the cell size of the grid by changing the grid's
// "Cell Size" Property.
void MyViz::setCellSize( int cell_size_percent )
{
  if( grid_ != NULL )
  {
    grid_->subProp( "Cell Size" )->setValue( cell_size_percent / 10.0f );
  }
}


<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/detection.h<|end_filename|>
#ifndef DETECTION_H
#define DETECTION_H

#include <QWidget>
#include <rviz/visualization_manager.h>
#include <rviz/render_panel.h>
#include <rviz/display.h>
#include <rviz/tool_manager.h>

#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>
#include <fight_with_hair_ui/main_window.hpp>

namespace Ui {
class detection;
}

class detection : public QWidget
{
  Q_OBJECT

public:
  explicit detection(bool isSim,QWidget *parent = 0);
  ~detection();

  void ros_init(int argc,char **argv);

  void setMain(fight_with_hair_ui::MainWindow*);

  void setEnvironment(bool);

public Q_SLOTS:
  void setObjectSize(int num);

private slots:
  void on_logout_clicked();

  void on_returnMain_clicked();

  void on_detect_start_clicked();

  void getAndSendObjectNum();

  void on_sendObjNum_clicked();

Q_SIGNALS:
  void sendObjectNum(int);
  void sendDetectMsg(bool);
  void detection_finish();

private:
  Ui::detection *ui;
  rviz::VisualizationManager *manager_;
  rviz::RenderPanel *render_panel_;
  int objectSize=0;
  fight_with_hair_ui::MainWindow *main;
  int choseObjNum=-1;
  bool ifIsSim = false;

protected:
  void closeEvent(QCloseEvent *event);
};

#endif // DETECTION_H


<|start_filename|>robot/src/slam/src/slam_lidar_filter.cpp<|end_filename|>

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/LaserScan.h>

static std::string pub_topic;
class LidarFilter
{
public:
    LidarFilter();
private:
     ros::NodeHandle n;
     ros::Publisher scan_pub;
     ros::Subscriber scan_sub;
     void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
};

LidarFilter::LidarFilter()
{
    // 将过滤后的消息发送到pub_tobic中（在launch文件中定义）
    scan_pub = n.advertise<sensor_msgs::LaserScan>(pub_topic,1);
    // 模拟器直接将雷达数据发送scan中
    scan_sub = n.subscribe<sensor_msgs::LaserScan>("/scan_raw",1,&LidarFilter::lidarCallback,this);
}

void LidarFilter::lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    int nRanges = scan->ranges.size();
    sensor_msgs::LaserScan new_scan;

    new_scan.header.stamp = scan->header.stamp;         // header 第一束激光
    new_scan.header.frame_id = scan->header.frame_id;
    new_scan.angle_max = scan->angle_max;               // 扫描的结束角度 
    new_scan.angle_min = scan->angle_min;               // 扫描的起始角度 
    new_scan.angle_increment = scan->angle_increment;   // 每次测量间的角度差
    new_scan.time_increment = scan->time_increment;     // 每次测量间的时间差

    new_scan.range_min = 0.25;                          // 距离最小值 0.25m
    new_scan.range_max = scan->range_max;               // 距离最大值
    new_scan.ranges.resize(nRanges);    
    new_scan.intensities.resize(nRanges);

    // 移除较远信息
    for(int i=0 ; i<nRanges ; i++)
    {
        new_scan.ranges[i] = scan->ranges[i];
        if(new_scan.ranges[i] < 0.25)
        {
            new_scan.ranges[i] = new_scan.range_max+1.0;
            if (new_scan.ranges[i] < 0.25) {
                new_scan.ranges[i] = 0.25;
            }
        }
        new_scan.intensities[i] = scan->intensities[i];
    }
    scan_pub.publish(new_scan);
}

int main(int argc, char** argv)
{
    ros::init(argc,argv,"slam_lidar_filter");

    ros::NodeHandle n_param("~");
    std::string strSerialPort;
    n_param.param<std::string>("pub_topic", pub_topic, "/fight_with_hair/slam/scan");

    LidarFilter lidar_filter;
    ros::spin();
}

<|start_filename|>robot/src/navi/test/test_navi_lidar_filter.cpp<|end_filename|>
#include <gtest/gtest.h>
#include <sensor_msgs/LaserScan.h>
#include <ros/ros.h>
sensor_msgs::LaserScan new_scan;

void lidarCallback(const sensor_msgs::LaserScan scan)
{
    int nRanges = scan.ranges.size();
    sensor_msgs::LaserScan new_scan;
    new_scan.range_max = scan.range_max;
    new_scan.range_min = 0.25;
    new_scan.ranges.resize(nRanges);
    new_scan.intensities.resize(nRanges);
    for(int i=0 ; i<nRanges ; i++)
    {
        new_scan.ranges[i] = scan.ranges[i];
        if(new_scan.ranges[i] < 0.25)
        {
            new_scan.ranges[i] = new_scan.range_max+1.0;
        }
        new_scan.intensities[i] = scan.intensities[i];
    }
}

int check(sensor_msgs::LaserScan scan)
{
    int nRange = scan.ranges.size();
    for (int i = 0; i < nRange; i++)
    {
        if (scan.ranges[i] < 0.25)
        {
            return 0;
        }
    }
    return 1;
}

TEST(lidar_filter_test, test1)
{
    sensor_msgs::LaserScan scan;
    scan.ranges.resize(5);
    scan.ranges[0] = 0.1;
    scan.ranges[1] = 1;
    scan.ranges[2] = 2;
    scan.ranges[3] = 3;
    scan.ranges[4] = 4;
    scan.range_max = 4;
    scan.range_min = 0;
    lidarCallback(new_scan);
    EXPECT_EQ(check(new_scan), 1);
}

TEST(lidar_filter_test, test2)
{
    sensor_msgs::LaserScan scan;
    scan.ranges.resize(5);
    scan.ranges[0] = 5;
    scan.ranges[1] = 4;
    scan.ranges[2] = 3;
    scan.ranges[3] = 2;
    scan.ranges[4] = 1;
    scan.range_max = 5;
    scan.range_min = 1;
    lidarCallback(new_scan);
    EXPECT_EQ(check(new_scan), 1);
}
TEST(lidar_filter_test, test3)
{
    sensor_msgs::LaserScan scan;
    scan.ranges.resize(1);
    scan.ranges[0] = 1;
    scan.range_max = 1;
    scan.range_min = 1;
    lidarCallback(new_scan);
    EXPECT_EQ(check(new_scan), 1);
}
TEST(lidar_filter_test, test4)
{
    sensor_msgs::LaserScan scan;
    scan.ranges.resize(5);
    scan.ranges[0] = 1.2;
    scan.ranges[1] = 4.2;
    scan.ranges[2] = 2.3;
    scan.ranges[3] = 5.4;
    scan.ranges[4] = 3.3;
    scan.range_max = 5.4;
    scan.range_min = 1.2;
    lidarCallback(new_scan);
    EXPECT_EQ(check(new_scan), 1);
}

TEST(lidar_filter_test, test5)
{
    sensor_msgs::LaserScan scan;
    scan.ranges.resize(5);
    scan.ranges[0] = 1.9;
    scan.ranges[1] = 2.8;
    scan.ranges[2] = 3.7;
    scan.ranges[3] = 4.6;
    scan.ranges[4] = 5.5;
    scan.range_max = 6.4;
    scan.range_min = 7.3;
    lidarCallback(new_scan);
    EXPECT_EQ(check(new_scan), 1);
}

int main(int argc, char **argv)
{
    srand(time(0));
    testing::InitGoogleTest(&argc, argv);

    return RUN_ALL_TESTS();
}


<|start_filename|>robot/src/fight_with_hair_ui/src/detection.cpp<|end_filename|>
#include "../include/fight_with_hair_ui/detection.h"
#include "ui_detection.h"
#include "../include/fight_with_hair_ui/mainframe.h"
#include <QMessageBox>
#include <QString>
#include <QCloseEvent>
#include <QListWidgetItem>
#include <QRadioButton>
#include <string>

detection::detection(bool isSim,QWidget *parent) :
  QWidget(parent),
  ui(new Ui::detection)
{
  ui->setupUi(this);
 //this->setFixedSize(this->geometry().width(),this->geometry().height());
  QString objStr;
  if (isSim){
    objStr = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch fwh_obj fwh_obj_t.launch'&";

  }else{
    objStr = "gnome-terminal -x bash -c 'source ~/.bashrc;roslaunch fwh_obj fwh_obj_l.launch'&";
  }
  system(objStr.toLatin1().data());
  ifIsSim = isSim;
  render_panel_ = new rviz::RenderPanel;
  ui->mapdisplay_layout->addWidget(render_panel_);
  manager_ = new rviz::VisualizationManager(render_panel_);

  render_panel_->initialize(manager_->getSceneManager(), manager_);
  manager_->initialize();
  //manager_->load(config);
  manager_->startUpdate();
  manager_->setFixedFrame("/base_footprint");
  //  setObjectSize(10);
}

detection::~detection()
{
  delete ui;
}

void detection::ros_init(int argc,char **argv)
{
  ros::init(argc,argv,"navigation",ros::init_options::AnonymousName);
}

void detection::on_logout_clicked()
{
  QMessageBox::StandardButton choice=QMessageBox::question(this,"确认","确定要退出吗？",QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes);
  if(choice==QMessageBox::Yes){
    this->close();
    QString string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /fwh_obj_detect;rosnode kill /fwh_obj_grab'&";
//    system(string.toLatin1().data());
    string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /joint_state_publisher;rosnode kill /robot_state_publisher;'&";
//    system(string.toLatin1().data());
    QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
    system(killString.toLatin1().data());
    killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
    system(killString.toLatin1().data());
    QApplication::quit();
  }
}

void detection::on_returnMain_clicked()
{
  //  mainframe *main=new mainframe;
  //  main->move(550,200);
  //  main->show();

  QString string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /fwh_obj_detect;rosnode kill /fwh_obj_grab'&";
//  system(string.toLatin1().data());
  QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
  system(killString.toLatin1().data());
  killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
  system(killString.toLatin1().data());
  this->close();
}


void detection::on_detect_start_clicked()
{

  manager_->removeAllDisplays();

  rviz::Display *grid_ = manager_->createDisplay( "rviz/Grid", "fwh_grid", true);
  ROS_ASSERT(grid_!=NULL);
  grid_->subProp( "Line Style" )->setValue("Billboards");

  //机器人模型
  rviz::Display *robot_ = manager_->createDisplay("rviz/RobotModel", "fwh_robot", true);
  ROS_ASSERT(robot_!=NULL);
  robot_->subProp("Robot Description")->setValue("robot_description");
  //相机
  rviz::Display *camera_ = manager_->createDisplay("rviz/PointCloud2", "fwh_camera", true);
  ROS_ASSERT(camera_!=NULL);
  if (ifIsSim){
    camera_->subProp("Topic")->setValue("/kinect2/sd/points");
  }else{
    camera_->subProp("Topic")->setValue("/kinect2/qhd/points");
  }
  camera_->subProp("Size (m)")->setValue("0.01");
  camera_->subProp("Alpha")->setValue("1");
  rviz::Display *marker_ = manager_->createDisplay("rviz/Marker", "fwh_marker", true);
  ROS_ASSERT(marker_!=NULL);
  marker_->subProp("Marker Topic")->setValue("/obj_marker");
  
  marker_->subProp("Queue Size")->setValue("100");

//  rviz::Display *kine_=manager_->createDisplay("rviz/Kinect", "fwh_kine", true);
//  ROS_ASSERT(kine_!=NULL);
//  kine_->subProp("Topic")->setValue("/kinect2/qhd/points");
//  kine_->subProp("Size (m)")->setValue("0.01");
//  kine_->subProp("Alpha")->setValue("1");

  manager_->startUpdate();

  emit sendDetectMsg(true);

}

void detection::setObjectSize(int num){
  choseObjNum=-1;
  objectSize=num;
  ui->object_list->clear();
  for (int i=0;i<num;i++)
  {
    QListWidgetItem *objItem = new QListWidgetItem();
    std::string str=std::to_string(i);
    std::string description = "Object" + str;
    const char *p = description.data();
    QRadioButton *box = new QRadioButton();
    box->setText(QString(p));
    ui->object_list->addItem(objItem);
    ui->object_list->setItemWidget(objItem, box);
    connect(box,&QRadioButton::clicked,this,&detection::getAndSendObjectNum);
  }
}

void detection::closeEvent(QCloseEvent *event){
  main->show();
  emit detection_finish();
}

void detection::setMain(fight_with_hair_ui::MainWindow *m){
  main=m;
}

void detection::getAndSendObjectNum(){
  QRadioButton *obj=(QRadioButton *)(sender());
  std::string str=obj->text().toStdString();
  str=str.substr(6,str.length());
  const char *p = str.data();
  choseObjNum=std::atoi(p);
}

void detection::on_sendObjNum_clicked()
{
  if (choseObjNum==-1){
    QMessageBox::question(this,"Warning","未选择物品",QMessageBox::Yes,QMessageBox::Yes);
    return;
  }
  emit sendObjectNum(choseObjNum);
  choseObjNum=-1;
}


<|start_filename|>robot/src/drivers/xfyun_waterplus/src/recorder_wav_node.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* @author <NAME>                                             */

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <alsa/asoundlib.h>
#include <string.h>
#include <signal.h>
#include <sound_play/SoundRequest.h>
#include "xfyun_waterplus/IATSwitch.h"

static ros::Publisher play_pub;
static int nRecDuring = 10;

#define BUFSIZE		(1024 * 4)

FILE *capture_file;
unsigned int size = 0;
snd_pcm_t *capture_handle;

void write_wav_header(unsigned char *header, uint16_t wFormatTag, uint16_t wChannels, 
			uint32_t dwSamplesPerSec, uint16_t wBitsPerSample)
{
	uint32_t dwAvgBytesPerSec;
	uint16_t wBlockAlign;

	/* writing riff */
	unsigned char riff[12] = { 'R', 'I', 'F', 'F',		/* RIFF flag */ 
	                     0 ,  0 ,  0 ,  0 ,		/* File size */
	                    'W', 'A', 'V', 'E' };	/* WAVE flag*/

	memcpy(&header[0], &riff[0], sizeof(riff));

	/* writing fmt */
	unsigned char fmt[24] = {  'f',  'm',  't',  ' ',	/* fmt flag */
	                   0x10, 0x00, 0x00, 0x00 };	/* fmt length */
	
	
	/* format, channels */
	fmt[8] = wFormatTag & 0xFF;
	fmt[9] = (wFormatTag >> 8) & 0xFF;
	fmt[10] = wChannels & 0xFF;
	fmt[11] = (wChannels >> 8) & 0xFF;

	/* sample rate */
	fmt[12] = dwSamplesPerSec & 0xFF;
	fmt[13] = (dwSamplesPerSec >> 8) & 0xFF;
	fmt[14] = (dwSamplesPerSec >> 16) & 0xFF;
	fmt[15] = (dwSamplesPerSec >> 24) & 0xFF;
	
	/* frame size */
	if (wFormatTag) 
		wBlockAlign = wChannels * ((wBitsPerSample + 7) / 8);
	else 
		wBlockAlign = wBitsPerSample / 8 * wChannels;

	fmt[20] = wBlockAlign & 0xFF;
	fmt[21] = (wBlockAlign >> 8) & 0xFF;
	fmt[22] = wBitsPerSample & 0xFF;
	fmt[23] = (wBitsPerSample >> 8) & 0xFF;
	
	/* bytes per second */
	dwAvgBytesPerSec = dwSamplesPerSec * wBlockAlign;
	fmt[16] = dwAvgBytesPerSec & 0xFF;
	fmt[17] = (dwAvgBytesPerSec >> 8) & 0xFF;
	fmt[18] = (dwAvgBytesPerSec >> 16) & 0xFF;
	fmt[19] = (dwAvgBytesPerSec >> 24) & 0xFF;
	

	memcpy(&header[12], &fmt[0], sizeof(fmt));
	
	/* writing data (header) */
	unsigned char data[8] = {  'd',  'a',  't',  'a',
	                   0x00, 0x00, 0x00, 0x00 };	/* data chunk size */

	memcpy(&header[36], &data[0], sizeof(data));
}

void update_wav_size(unsigned char *file, int file_len)
{
	file[7] = (file_len >> 24) & 0xFF;
	file[6] = (file_len >> 16) & 0xFF;
	file[5] = (file_len >> 8 ) & 0xFF,
	file[4] =  file_len & 0xFF; 

	int data_len = file_len - 44;
	file[43] = (data_len >> 24) & 0xFF;
	file[42] = (data_len >> 16) & 0xFF;
	file[41] = (data_len >> 8 ) & 0xFF,
	file[40] =  data_len & 0xFF; 
}

void file_update_wav_size()
{
	unsigned char buf[4];
	buf[3] = (size >> 24) & 0xFF;
	buf[2] = (size >> 16) & 0xFF;
	buf[1] = (size >> 8 ) & 0xFF,
	buf[0] =  size & 0xFF; 

	fseek(capture_file, 4, SEEK_SET);
	fwrite(&buf, sizeof(buf), 1, capture_file);

	int data_len = size - 44;
	buf[3] = (data_len >> 24) & 0xFF;
	buf[2] = (data_len >> 16) & 0xFF;
	buf[1] = (data_len >> 8 ) & 0xFF,
	buf[0] =  data_len & 0xFF; 

	fseek(capture_file, 40, SEEK_SET);
	fwrite(&buf, sizeof(buf), 1, capture_file);
}

void stop(int sig) {
	file_update_wav_size();
	fclose(capture_file);
	//snd_pcm_close (capture_handle);

	fprintf(stdout, "exited...\n");
	exit(0);
}

static bool bActive = false;
bool sr_switch(xfyun_waterplus::IATSwitch::Request  &req, xfyun_waterplus::IATSwitch::Response &res)
{
	bActive = req.active;
	if(bActive == true)
	{
		nRecDuring = req.duration;
		printf("[IATSwitch] 语音识别开启! 识别时长为 %d 秒\n",nRecDuring);
	}
	else
	{
		printf("[IATSwitch] 语音识别关闭!\n");
	}
	return true;
}

int main(int argc, char *argv[])
{
	ros::init(argc, argv, "waterplus_recorder_wav");

	char const* home_dir = getenv("HOME");
	std::string strHomeDir = home_dir;
	std::string strTonePlay = strHomeDir + "/catkin_ws/src/xfyun_kinetic/sound/on.wav";

	int ret;
	short buf[BUFSIZE * 2];

	//char device[] = "hw:1,0";
	char device[] = "default";
	snd_pcm_format_t fmt = SND_PCM_FORMAT_S16_LE;
	//snd_pcm_format_t fmt = SND_PCM_FORMAT_U8;
	int rate = 44100;
	int channels = 2;
	int bits = 16;

	char file_name[] = "/dev/shm/out.wav";

	int c;
	extern char *optarg;
	extern int optopt, optind;
	int f_set = 0;
	int r_set = 0;
	int d_set = 0;

	unsigned char wav_header[44];

	if ((ret = snd_pcm_open (&capture_handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0) 
	{
		printf ("Cannot open audio device %s (%s)\n", device,snd_strerror(ret));
		exit(1);
	} 

	if ((ret = snd_pcm_set_params(capture_handle, fmt,
					SND_PCM_ACCESS_RW_INTERLEAVED,
					channels,
					rate,
					1,
					500000)) < 0 ) 
	{
		printf("Playback open error (%s)\n", snd_strerror(ret));
		snd_pcm_close (capture_handle);
		exit(1);
	}

	if ((ret = snd_pcm_prepare (capture_handle)) < 0) 
	{
		fprintf(stderr, "Cannot prepare pcm for use (%s)\n", 
			snd_strerror(ret));
		snd_pcm_close (capture_handle);
		exit(1);
	}

	if ((ret = snd_pcm_start (capture_handle)) < 0) 
	{
		fprintf(stderr, "Cannot prepare pcm for use (%s)\n", 
			snd_strerror(ret));
		snd_pcm_close (capture_handle);
		exit(1);
	}

	ros::NodeHandle n;
	ros::Publisher snd_pub = n.advertise<std_msgs::String>("/xfyun/snd_file", 10);
    play_pub = n.advertise<sound_play::SoundRequest>("/robotsound", 20);
    ros::ServiceServer switch_svr = n.advertiseService("xfyun_waterplus/IATSwitch", sr_switch);
	ros::NodeHandle n_param("~");
    n_param.param<bool>("start", bActive, true);

	ros::Rate loop_rate(10);

	sleep(1);
	int count = 0;
	while (ros::ok())
	{
		if(bActive == true)
		{
			ROS_INFO("new record");
			std_msgs::String msg;
			std::stringstream ss;
			//ss << "/home/robot/speech_" << count << ".wav";
			ss << "/dev/shm/speech_" << count << ".wav";
			
			memset(buf, 0, sizeof(buf));

			capture_file = fopen(ss.str().c_str(), "w+");

			if (capture_file == NULL) {
				fprintf(stderr, "Cannot open file %s\n", file_name);
				snd_pcm_close (capture_handle);
				exit(1);
			}

			write_wav_header(wav_header, 1, channels, rate, bits);
			fwrite(&wav_header, sizeof(wav_header), 1, capture_file);

			signal(SIGINT, stop);

			//提示音
			sound_play::SoundRequest sp;
			sp.sound = sound_play::SoundRequest::PLAY_FILE;
			sp.command = sound_play::SoundRequest::PLAY_ONCE;
			sp.arg = strTonePlay;
			play_pub.publish(sp);
			usleep(1000*1000);

			while(1) 
			{
				if(bActive == false)
					break;
				int avail = snd_pcm_avail_update (capture_handle);
				if (avail > 0) 
				{
					if (avail > BUFSIZE);
						avail = BUFSIZE;
					snd_pcm_readi (capture_handle, buf, avail);
					fwrite(&buf, sizeof(buf), 1, capture_file);
					size += sizeof(buf);
					ROS_INFO("size = %d ",size);
					if(size >= nRecDuring*1024*1024/6)		//录音时长
						break;
				}
				else
				{
					printf("音频数据流错误: avail = %d \n",avail);
				}
			}
			if(bActive == false)
			{
				size = 0;
				continue;
			}

			file_update_wav_size();
			fclose(capture_file);
			size = 0;
			
			msg.data = ss.str();
			ROS_INFO("%s", msg.data.c_str());
			snd_pub.publish(msg);
			ros::spinOnce();

			loop_rate.sleep();
			++count;
			if(count > 10)
				count = 0;
		}
		else
		{
			//将数据读出来并抛弃
			int avail = snd_pcm_avail_update (capture_handle);
			if (avail > 0) 
			{
				if (avail > BUFSIZE);
						avail = BUFSIZE;
			 	snd_pcm_readi (capture_handle, buf, avail);
			}
			loop_rate.sleep();
			ros::spinOnce();
		}
	}

	snd_pcm_close (capture_handle);

	return 0;
}

<|start_filename|>robot/src/drivers/wpb_home_apps/src/scripts/home_script.cpp<|end_filename|>
#include "10_home_script.h"
CHomeScript::CHomeScript()
{
    
}

CHomeScript::~CHomeScript()
{

}

void CHomeScript::Queue()
{
    stAct newAct;

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "科大讯飞语音交互 Demo";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_LISTEN;
    newAct.strTarget = "机器人";
    newAct.nDuration = 5;
    arAct.push_back(newAct);
    
    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "你好,已经跟着你了,主人";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_FOLLOW;
    newAct.fFollowDist = 0.5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_LISTEN;
    newAct.strTarget = "我渴了";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "好的,主人你想来瓶饮料吗";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_LISTEN;
    newAct.strTarget = "是的";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "好的,这就给您去取";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_ADD_WAYPOINT;
    newAct.strTarget = "follow";
    arAct.push_back(newAct);

    newAct.nAct = ACT_GOTO;
    newAct.strTarget = "kitchen";
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "开始抓取饮料";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_GRAB;
    newAct.strTarget = "";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "饮料抓取完毕";
    newAct.nDuration = 2;
    arAct.push_back(newAct);

    newAct.nAct = ACT_GOTO;
    newAct.strTarget = "follow";
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "饮料已取回,主人请慢用";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_PASS;
    newAct.strTarget = "";
    newAct.nDuration = 5;
    arAct.push_back(newAct);

    newAct.nAct = ACT_SPEAK;
    newAct.strTarget = "任务完成";
    newAct.nDuration = 5;
    arAct.push_back(newAct);
}

<|start_filename|>robot/src/fight_with_hair_ui/src/navigation.cpp<|end_filename|>
#include "../include/fight_with_hair_ui/navigation.h"
#include "ui_navigation.h"
#include "../include/fight_with_hair_ui/mainframe.h"
#include <QMessageBox>
#include <QFileDialog>
#include <QListWidgetItem>
#include <QCheckBox>
#include <QCloseEvent>
#include "tinyxml.h"

navigation::navigation(bool isS,QWidget *parent) :
  QWidget(parent),
  ui(new Ui::navigation)
{
  ui->setupUi(this);
//  this->setFixedSize(this->geometry().width(),this->geometry().height());
  ui->waypoints_list->horizontalHeader()->setStretchLastSection(true);
  ui->waypoints_list->setEditTriggers(QAbstractItemView::NoEditTriggers);
  mapfile = "";
  mapname = "";
  xmlfile = "";

  isSim = isS;

  render_panel_ = new rviz::RenderPanel();
  ui->mapdisplay_layout->addWidget(render_panel_);
  manager_ = new rviz::VisualizationManager(render_panel_);
  tool_manager_ = manager_->getToolManager();
  render_panel_->initialize(manager_->getSceneManager(), manager_);
  manager_->initialize();
  manager_->startUpdate();
}

navigation::~navigation()
{
  delete ui;
}


void navigation::ros_init(int argc,char **argv)
{
  ros::init(argc,argv,"navigation",ros::init_options::AnonymousName);
}


void navigation::on_logout_clicked()
{
  QMessageBox::StandardButton choice=QMessageBox::question(this,"确认","确定要退出吗？",QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes);
  if(choice==QMessageBox::Yes){
    this->close();
//    QString string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /amcl;"
//                     "rosnode kill /map_server;"
//                     "rosnode kill /move_base;"
//                     "rosnode kill /slam_lidar_filter;"
//                     "rosnode kill /wp_manager;"
//                     "rosnode kill /wp_nav_test;"
//                     "rosnode kill /robot_state_publisher;'&";

//    system(string.toLatin1().data());
//    string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /joint_state_publisher;rosnode kill /robot_state_publisher;'&";
//    system(string.toLatin1().data());
    QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
    system(killString.toLatin1().data());
    killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
    system(killString.toLatin1().data());
    QApplication::quit();
  }
}

void navigation::on_returnMain_clicked()
{
  //  mainframe *main=new mainframe;
  //  main->move(550,200);
  //  main->show();
  QString string = "gnome-terminal -x bash -c 'source ~/.bashrc;rosnode kill /amcl;"
                   "rosnode kill /map_server;"
                   "rosnode kill /move_base;"
                   "rosnode kill /slam_lidar_filter;"
                   "rosnode kill /wp_manager;"
                   "rosnode kill /wp_nav_test;'&";

//  system(string.toLatin1().data());
  QString killString = "rosnode list | awk '{print $1}' | grep -v 'fight_with_hair_ui' | xargs rosnode kill";
  system(killString.toLatin1().data());
  killString = "ps -a | grep roslaunch | awk '{if (NR >= 2) {print $1}}' | xargs kill -9&";
  system(killString.toLatin1().data());
  this->close();
}

void navigation::on_display_button_clicked()
{
  if (mapfile == ""){
    QMessageBox::StandardButton choice=QMessageBox::question(this,"Warning","未选择地图，是否加载默认地图？",QMessageBox::Yes|QMessageBox::No,QMessageBox::Yes);
    if(choice==QMessageBox::No){
      return;
    }
  }
  else if (mapfile.right(mapfile.length() - mapfile.lastIndexOf(".") - 1) != "yaml"){
    QMessageBox::information(this, tr("ERROR"), tr("地图文件后缀名应为yaml"));
    return;
  }
  mapname = mapfile.mid(mapfile.lastIndexOf("/") + 1, mapfile.lastIndexOf(".") - mapfile.lastIndexOf("/") - 1);

  manager_->setFixedFrame("/map");
  manager_->removeAllDisplays();

  rviz::Display *map_ = manager_->createDisplay("rviz/Map", "fwh_ui map", true);
  ROS_ASSERT(map_!=NULL);
  map_->subProp("Topic")->setValue("/map");

  rviz::Display *robot_ = manager_->createDisplay("rviz/RobotModel", "fwh_ui robot", true);
  ROS_ASSERT(robot_!=NULL);
  robot_->subProp("Robot Description")->setValue("robot_description");

  rviz::Display *laser_=manager_->createDisplay("rviz/LaserScan","fwh_ui laser",true);
  ROS_ASSERT(laser_!=NULL);
  laser_->subProp("Topic")->setValue("/scan");
  laser_->subProp("Size (m)")->setValue("0.1");

  rviz::Display *path1_=manager_->createDisplay("rviz/Path","fwh_ui_path_1",true);
  ROS_ASSERT(path1_!=NULL);
  path1_->subProp("Topic")->setValue("/move_base/GlobalPlanner/plan");
  path1_->subProp("Line Style")->setValue("Billboards");
  path1_->subProp("Color")->setValue("255; 85; 255");
  path1_->subProp("Alpha")->setValue("1");
  path1_->subProp("Buffer Length")->setValue("1");


  rviz::Display *mark_ = manager_->createDisplay("rviz/Marker", "fwh_ui mark", true);
  if (mark_!=NULL)
  {
    ROS_ASSERT(mark_!=NULL);
    mark_->subProp("Marker Topic")->setValue("/waypoints_marker");
  }

  ui->waypoints_list->clearContents();
  int row = 0;
  int i=0;
  TiXmlDocument doc;
  QString xmlQStr="~/fight_with_hair/navi/waypoints/"+mapname;
  const char* xmlf=xmlQStr.toLatin1().data();
  if (doc.LoadFile(xmlf))
  {
    //    ui->map_name->setText("QString(p)");
    TiXmlElement *root = doc.FirstChildElement();
    for (TiXmlElement *elem = root->FirstChildElement(); elem != NULL; elem=elem->NextSiblingElement())
    {
      i++;
      // 获取元素名
      std::string elemName = elem->Value();
      const char *p = elemName.data();
      if(i==1){ui->map_name->setText(QString(p));}
      if (elemName == "Name"){
        TiXmlNode *e1 = elem->FirstChild();
        ui->waypoints_list->insertRow(ui->waypoints_list->rowCount());
        ui->waypoints_list->setItem(row, 0, new QTableWidgetItem(QString(e1->ToText()->Value())));
        row++;
      }
    }
  }
  QString mapShow;
  if (isSim){
    mapShow = "gnome-terminal -x bash -c 'source ~/.bashrc; roslaunch fight_with_hair_ui navigation_sim.launch map_file:=" + mapfile + " waypoint_name:="+ mapname + "'&";
  }else{
    mapShow = "gnome-terminal -x bash -c 'source ~/.bashrc; roslaunch fight_with_hair_ui navigation_gen.launch map_file:=" + mapfile + " waypoint_name:="+ mapname + "'&";
  }
  system(mapShow.toLatin1().data());

}

void navigation::on_chooseMap_clicked()
{
  mapfile = QFileDialog::getOpenFileName(this, tr("选择地图"), "..", "", 0);
  QString mapname = "地图:" + mapfile.right(mapfile.length() - mapfile.lastIndexOf("/") - 1);
  if (mapfile == "")
  {
    QMessageBox::information(this, tr("WARNING"), tr("未选择地图"));
    ui->map_name->setText("请选择正确地图");
  }
  else if (mapfile.right(mapfile.length() - mapfile.lastIndexOf(".") - 1) != "yaml")
  {
    QMessageBox::information(this, tr("WARNING"), tr("地图格式应为yaml"));
    ui->map_name->setText("请选择正确地图");
  }else{
    ui->map_name->setText(mapname);
  }
}

void navigation::getButtonText(bool check)
{

}


void navigation::on_chooseDes_clicked()
{
  current_tool_ = tool_manager_->addTool("rviz/SetGoal");
  rviz::Property *pro = current_tool_->getPropertyContainer();
  pro->subProp("Topic")->setValue("move_base_simple/goal");
  manager_->setFixedFrame("map");
  tool_manager_->setCurrentTool(current_tool_);
}

void navigation::on_chooseWayPoint_clicked()
{
  current_tool_ = tool_manager_->addTool("rviz/AddWaypoint");
  rviz::Property *pro = current_tool_->getPropertyContainer();
  pro->subProp("Topic")->setValue("/waterplus/add_waypoint");
  manager_->setFixedFrame("map");
  tool_manager_->setCurrentTool(current_tool_);

  manager_->startUpdate();
}

void navigation::closeEvent(QCloseEvent *event)
{
  main->show();
  emit navigation_finish();
}

void navigation::on_cruise_button_clicked()
{
  QString cruise = "gnome-terminal -x bash -c 'source ~/.bashrc;rosrun waterplus_map_tools wp_nav_test'&";
  system(cruise.toLatin1().data());
}

void navigation::on_save_waypoints_clicked()
{
  QString saveWaypoint = "gnome-terminal -x bash -c 'source ~/.bashrc;rosrun navi way_saver ~/fight_with_hair/src/navi/waypoints/"+mapname+".xml'&";
  system(saveWaypoint.toLatin1().data());
  QMessageBox::information(this, tr("提示"), tr("保存成功!"));
}

void navigation::setMain(fight_with_hair_ui::MainWindow *m){
  main=m;
}


<|start_filename|>robot/src/drivers/wpb_home/wpb_home_tutorials/src/wpb_home_serve_drinks.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* @author <NAME>                                             */

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <wpb_home_behaviors/Coord.h>
#include <sensor_msgs/JointState.h>

static ros::Publisher behaviors_pub;
static std_msgs::String behavior_msg;
static ros::Publisher waypoint_pub;
static ros::Publisher grab_drink_pub;
static geometry_msgs::Pose grab_drink_msg;
static ros::Publisher mani_ctrl_pub;
static sensor_msgs::JointState mani_ctrl_msg;

#define STEP_READY                                      0
#define STEP_GOTO_KITCHEN                   1
#define STEP_DRINK_DETECT                    2
#define STEP_GRAB_DRINK                         3
#define STEP_GOTO_DINNING_ROOM    4
#define STEP_PUT_DOWN                            5
#define STEP_BACKWARD                             6
#define STEP_DONE                                         7
int nStep = STEP_READY;
static int nDeley = 0;

void DrinkCoordCB(const wpb_home_behaviors::Coord::ConstPtr &msg)
{
    if(nStep == STEP_DRINK_DETECT)
    {
        // 获取饮料检测结果
        int drink_num = msg->name.size();
        ROS_INFO("[DrinkCoordCB] drink_num = %d",drink_num);
        for(int i = 0;i<drink_num;i++)
        {
            ROS_INFO("[DrinkCoordCB]  %s  (%.2f , %.2f , %.2f)",msg->name[i].c_str(),msg->x[i],msg->y[i],msg->z[i]);
        }
        int grab_drink_index = 0;
        grab_drink_msg.position.x = msg->x[grab_drink_index];
        grab_drink_msg.position.y = msg->y[grab_drink_index];
        grab_drink_msg.position.z = msg->z[grab_drink_index];
        grab_drink_pub.publish(grab_drink_msg);

        std_msgs::String behavior_msg;
        behavior_msg.data = "object_detect stop";
        behaviors_pub.publish(behavior_msg);
        nStep = STEP_GRAB_DRINK;
    }
}

void NaviResultCB(const std_msgs::String::ConstPtr &msg)
{
    if(nStep == STEP_GOTO_KITCHEN)
    {
        if(msg->data == "done")
        {
            ROS_INFO("[NaviResultCB] Waypoint kitchen!");
            behavior_msg.data = "object_detect start";
            behaviors_pub.publish(behavior_msg);
            nStep = STEP_DRINK_DETECT;
        }
    }
     
    if(nStep == STEP_GOTO_DINNING_ROOM)
    {
        if(msg->data == "done")
        {
            ROS_INFO("[NaviResultCB] Waypoint dinning room!");
            mani_ctrl_msg.name[0] = "gripper";
            mani_ctrl_msg.position[0] = 0.15;   //张开手爪
            mani_ctrl_pub.publish(mani_ctrl_msg);
            nDeley = 0;
            nStep = STEP_PUT_DOWN;
        }
    }
}

void GrabResultCB(const std_msgs::String::ConstPtr &msg)
{
    if(nStep == STEP_GRAB_DRINK)
    {
        if(msg->data == "done")
        {
            ROS_INFO("[GrabResultCB] grab_drink done!");
            std_msgs::String waypoint_msg;
            waypoint_msg.data = "dinning room";
            waypoint_pub.publish(waypoint_msg);
            nStep = STEP_GOTO_DINNING_ROOM;
        }
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "wpb_home_drink_serve");

    ros::NodeHandle n;
    behaviors_pub = n.advertise<std_msgs::String>("/wpb_home/behaviors", 10);
    ros::Subscriber drink_result_sub = n.subscribe("/wpb_home/objects_3d", 10 , DrinkCoordCB);
    waypoint_pub = n.advertise<std_msgs::String>("/waterplus/navi_waypoint", 10);
    ros::Subscriber navi_res_sub = n.subscribe("/waterplus/navi_result", 10, NaviResultCB);
    grab_drink_pub = n.advertise<geometry_msgs::Pose>("/wpb_home/grab_action", 1);
    ros::Subscriber grab_res_sub = n.subscribe("/wpb_home/grab_result", 10, GrabResultCB);
    mani_ctrl_pub = n.advertise<sensor_msgs::JointState>("/wpb_home/mani_ctrl", 10);
    ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    mani_ctrl_msg.name.resize(1);
    mani_ctrl_msg.position.resize(1);
    mani_ctrl_msg.velocity.resize(1);
    mani_ctrl_msg.name[0] = "lift";
    mani_ctrl_msg.position[0] = 0;
    
    sleep(1);

    ros::Rate r(10);
    while(ros::ok())
    {
        if(nStep == STEP_READY)
        {
            std_msgs::String waypoint_msg;
            waypoint_msg.data = "kitchen";
            waypoint_pub.publish(waypoint_msg);
            nStep = STEP_GOTO_KITCHEN;
        }

        if(nStep == STEP_PUT_DOWN)
        {
            nDeley ++;
            if(nDeley > 5*10)
            {
                nDeley = 0;
                nStep = STEP_BACKWARD;
            }
        }

        if(nStep == STEP_BACKWARD)
        {
            geometry_msgs::Twist vel_cmd;
            vel_cmd.linear.x = -0.1;
            vel_pub.publish(vel_cmd);
            nDeley ++;
            if(nDeley > 5 *10)
            {
                mani_ctrl_msg.name[0] = "lift";
                mani_ctrl_msg.position[0] = 0;            //收回手臂
                mani_ctrl_pub.publish(mani_ctrl_msg);

                vel_cmd.linear.x = 0;                               //停止移动
                vel_pub.publish(vel_cmd);
                nStep = STEP_DONE;
            }
        }

        ros::spinOnce();
        r.sleep();
    }
    
    return 0;
}


<|start_filename|>robot/src/drivers/wpb_home/wpb_home_bringup/src/wpb_home_calibrate_velocity.cpp<|end_filename|>
/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*!******************************************************************
 @author     ZhangWanjie
 ********************************************************************/
 
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <std_msgs/String.h>

class CVelocityTest
{
public:
  ros::NodeHandle nh_;
  ros::Publisher cmd_vel_pub_;
  ros::Publisher odom_ctrl_pub;
  ros::Subscriber pose_diff_sub;
  geometry_msgs::Pose2D pose_diff;

  //! ROS node initialization
  CVelocityTest(ros::NodeHandle &nh)
  {
    nh_ = nh;
    //set up the publisher for the cmd_vel topic
    cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
    odom_ctrl_pub = nh.advertise<std_msgs::String>("/wpb_home/ctrl", 30);
    pose_diff_sub = nh.subscribe("/wpb_home/pose_diff", 1, &CVelocityTest::PoseDiffCallback,this);
  }

  void PoseDiffCallback(const geometry_msgs::Pose2D::ConstPtr& msg)
  {
      pose_diff.x = msg->x;
      pose_diff.y = msg->y;
      pose_diff.theta = msg->theta;
  }

  //! Loop forever while sending drive commands based on keyboard input
  bool driveKeyboard()
  {
    std::cout << "输入如下指令并回车执行：\n"
      "W - 前进1米 \nS - 后退1米\n"
      "A - 左平移1米\nD - 右平移1米 \n"
      "Q - 逆时针转360度\nE - 顺时针转360度\nX - 退出\n";

    //we will be sending commands of type "twist"
    geometry_msgs::Twist base_cmd;
    ros::Rate r(2);

    char cmd[50];
    while(nh_.ok())
    {
      std::cin.getline(cmd, 50);
      if(cmd[0]!='w' && cmd[0]!='a' && cmd[0]!='s' && cmd[0]!='d' && cmd[0]!='q' && cmd[0]!='e' && cmd[0]!='x')
      {
        std::cout << "无效指令：" << cmd << "\n";
        continue;
      }
      else
      {
        std_msgs::String odom_ctrl_msg;
        odom_ctrl_msg.data = "pose_diff reset";
        odom_ctrl_pub.publish(odom_ctrl_msg);
        ros::spinOnce();
        std::cout << "准备执行指令：" << cmd << "\n";
        r.sleep();
      }

      int nDelay = 0;
      base_cmd.linear.x = base_cmd.linear.y = base_cmd.angular.z = 0;   
      //move forward
      if(cmd[0]=='w')
      {
        base_cmd.linear.x = 0.25;
        nDelay = 4;
      } 
      if(cmd[0]=='s')
      {
        base_cmd.linear.x = -0.25;
        nDelay = 4;
      } 
      //shift left 
      else if(cmd[0]=='a')
      {
        base_cmd.linear.y = 0.25;
        nDelay = 4;
      
      } 
      //shift right 
      else if(cmd[0]=='d')
      {
        base_cmd.linear.y = -0.25;
        nDelay = 4;
      } 
      //turn left (yaw) 
      else if(cmd[0]=='q')
      {
        base_cmd.angular.z = 3.1415926/4; 
        nDelay = 8;
      } 
      //turn right (yaw) 
      else if(cmd[0]=='e')
      {
        base_cmd.angular.z = -3.1415926/4;
        nDelay = 8;
      } 
      //quit
      else if(cmd[0]=='x')
      {
        break;
      }

      while(nDelay > 0)
      {
          std::cout << "倒计时 = " << nDelay << "\n";
          nDelay --;
          //publish the assembled command
          cmd_vel_pub_.publish(base_cmd);
          sleep(1);     
      }
      
      base_cmd.linear.x = 0;
      base_cmd.linear.y = 0;
      base_cmd.angular.z = 0;
      cmd_vel_pub_.publish(base_cmd);
      ros::spinOnce();
      r.sleep();
      
      std::cout << "任务完成!\n";
      std::cout << "-------------------------\n";
      printf("**电机码盘里程计**\n前后运动= %.2f 米\n左右平移= %.2f 米\n转动角度= %.2f 度\n",pose_diff.x,pose_diff.y,pose_diff.theta*180/3.1415926);
     std_msgs::String odom_ctrl_msg;
      odom_ctrl_msg.data = "motor encoder";
      odom_ctrl_pub.publish(odom_ctrl_msg);
      ros::spinOnce();
      std::cout << "-------------------------\n";
      std::cout << "\n";
      std::cout << "输入下一条指令：";
    }
    return true;
  }

};

int main(int argc, char** argv)
{
  //init the ROS node
  ros::init(argc, argv, "wpb_home_calibrate_velocity");
  ros::NodeHandle nh;

  CVelocityTest vt(nh);
  vt.driveKeyboard();

}


<|start_filename|>robot/src/fight_with_hair_ui/include/fight_with_hair_ui/build_map_auto.h<|end_filename|>
#ifndef BUILD_MAP_AUTO_H
#define BUILD_MAP_AUTO_H

#include <QWidget>
#include <rviz/visualization_manager.h>
#include <rviz/render_panel.h>
#include <rviz/display.h>
#include <rviz/tool_manager.h>

#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>
#include <fight_with_hair_ui/main_window.hpp>

namespace Ui {
class build_map_auto;
}

class build_map_auto : public QWidget
{
  Q_OBJECT

public:
  explicit build_map_auto(bool isSim,QWidget *parent = 0);
  ~build_map_auto();

  void setMain(fight_with_hair_ui::MainWindow *);

Q_SIGNALS:
  void build_map_auto_finish();

private slots:
  void on_add_map_button_clicked();

  void on_returnMain_clicked();

  void on_logout_clicked();

  void on_addNaviGoal_clicked();

  void on_addPublishPoint_clicked();

  void on_map_show_btn_clicked();

private:
  Ui::build_map_auto *ui;
  rviz::VisualizationManager *manager_;
  rviz::RenderPanel *render_panel_;
  rviz::Display *map_;
  QTimer *m_timer;
  rviz::Tool *current_tool_;
  rviz::ToolManager *tool_manager_;
  bool hasSaved=false;
  fight_with_hair_ui::MainWindow *main;
  bool isSim=false;

protected:
  void closeEvent(QCloseEvent *event);

};

#endif // BUILD_MAP_AUTO_H


<|start_filename|>robot/src/fwh_follow/src/fwh_follow_yz.cpp<|end_filename|>
#include <ros/ros.h>
#include <std_msgs/UInt32.h>
#include <wpb_home_tutorials/Follow.h>

static ros::ServiceClient begin_client;
static ros::ServiceClient stop_client;
static bool ifdoing = true;


void follow_begin(const std_msgs::UInt32::ConstPtr & msg){
    if (ifdoing == false) {
        ROS_WARN("get follow begin signal");
        wpb_home_tutorials::Follow tp;
        tp.request.thredhold = 0.6;
        begin_client.call(tp);
        ifdoing = true;
    }
}

void follow_stop(const std_msgs::UInt32::ConstPtr & msg){
    if (ifdoing == true) {
        ROS_WARN("get follow stop signal");
        wpb_home_tutorials::Follow tp;
        tp.request.thredhold = 0.6;
        stop_client.call(tp);
        ifdoing = false;
    }
}

int main(int argc, char** argv){

    ros::init(argc, argv, "fwh_follow_yz");
    ROS_WARN("fwh_follow_yz start!");
    ros::NodeHandle nh;
    ros::Subscriber begin_sub = nh.subscribe("/fight_with_hair/uito/follow_begin", 1, follow_begin);
    begin_client = nh.serviceClient<wpb_home_tutorials::Follow>("wpb_home_follow/start");
    ros::Subscriber stop_sub = nh.subscribe("/fight_with_hair/uito/follow_stop", 1, follow_stop);
    stop_client = nh.serviceClient<wpb_home_tutorials::Follow>("wpb_home_follow/stop");
    ros::spin();
    return 0;
}


<|start_filename|>robot/src/arm/src/arm_move.cpp<|end_filename|>
#include<ros/ros.h>
#include<sensor_msgs/JointState.h>
#include"arm/armMsg.h"
#include<algorithm>
enum stateNumber
{
    mani_zero,
    mani_work
};


using namespace std;

#define MAXLEN 1.0
#define MINLEN 0.5
#define MAXDEGREE 0.1
#define MINDEGREE 0.0

static int state = mani_zero;
sensor_msgs::JointState ctrl_msg;
ros::Subscriber sub;
ros::Publisher pub;


float checkLen(float direct, float len){
    if (direct == 1){ //up
        return min(ctrl_msg.position[0] + len,MAXLEN);
    }
    else if (direct == 2){ //down
        return max(MINLEN, ctrl_msg.position[0] - len);
    }
    else{
        return ctrl_msg.position[0];
    }
}

float checkDegree(float direct, float degree){
    if (direct == 2){ //fold
        return min(ctrl_msg.position[1] + degree,MAXDEGREE);
    }
    else if (direct == 1){ //open
        return max(MINDEGREE, ctrl_msg.position[1] - degree);
    }
    else{
        return ctrl_msg.position[1];
    }
}

void arm_listen(const arm::armMsg::ConstPtr& msg){
    //set v
  if (msg->direction_h == 0 && msg->direction_v == 0){
    state = mani_zero;
    ctrl_msg.position[0] = 0;
    ctrl_msg.velocity[0] = 0.5;
    ctrl_msg.position[1] = 0.1;
    ctrl_msg.velocity[1] = 5;
    pub.publish(ctrl_msg); 
  }
  else if (state==mani_zero && msg->direction_v == 1){
    ctrl_msg.position[0] = 0.5; //(0.5m)
    ctrl_msg.velocity[0] = 0.5; //(0.5m/s)
    ctrl_msg.position[1] = 0;
    ctrl_msg.velocity[1] = 5;    //(5 degree/s)
    state = mani_work;
    pub.publish(ctrl_msg);
  }
  else if (state==mani_work){
    if (msg->direction_v == 0 && msg->direction_h == 0){
       ctrl_msg.position[0] = 0; //(0.5m)
       ctrl_msg.velocity[0] = 0.5; //(0.5m/s)
       ctrl_msg.position[1] = 0.1;
       ctrl_msg.velocity[1] = 5;    //(5 degree/s)
       state = mani_zero;
       pub.publish(ctrl_msg);
       return;
    }

    if (msg->direction_v == 0){
        ctrl_msg.velocity[0] = 0;
    }else{
        ctrl_msg.velocity[0] = 0.5;    
    }

    if (msg->direction_h == 0){
        ctrl_msg.velocity[1] = 0;
    }else{
        ctrl_msg.velocity[1] = 5;
    }
    
    ctrl_msg.position[0] = checkLen(msg->direction_v, msg->length);
    ctrl_msg.position[1] = checkDegree(msg->direction_h, msg->degree);

    ROS_INFO("p1:%.6f,v1:%.6f;p2:%.6f,v2:%.6f",ctrl_msg.position[0],ctrl_msg.velocity[0],ctrl_msg.position[1],ctrl_msg.velocity[1]);

    pub.publish(ctrl_msg);
  }
}


int main(int argc, char** argv){
    ros::init(argc, argv, "arm_move");
    ros::NodeHandle nh;

    //init
    ctrl_msg.name.resize(2);
    ctrl_msg.position.resize(2);
    ctrl_msg.velocity.resize(2);
    ctrl_msg.name[0] = "lift";  //up and down
    ctrl_msg.name[1] = "gripper"; //grab
    ctrl_msg.position[0] = 0;
    ctrl_msg.position[1] = 0;
    pub = nh.advertise<sensor_msgs::JointState>("/wpb_home/mani_ctrl",30);
    sleep(2);

    sub = nh.subscribe("/fight_with_hair/arm_move", 10, arm_listen);

    int cnt = 0;
    ros::Rate loop_rate(10);

    while(ros::ok()){
        // if (state == mani_zero){
        //     ROS_INFO("ZERO->DOWN");
        //     ctrl_msg.position[0] = 0.5; //(0.5m)
        //     ctrl_msg.velocity[0] = 0.5; //(0.5m/s)
        //     ctrl_msg.position[1] = 0.1; 
        //     ctrl_msg.velocity[1] = 5;    //(5 degree/s)
        //     state = mani_down;
        // }
        // else if (state == mani_down){
        //     ROS_INFO("DOWN->UP");
        //     ctrl_msg.position[0] = 1; //(0.5m)
        //     ctrl_msg.velocity[0] = 0.5; //(0.5m/s)
        //     ctrl_msg.position[1] = 0; 
        //     ctrl_msg.velocity[1] = 5;    //(5 degree/s)
        //     state = mani_up;
        // }
        // else if (state == mani_up){
        //     ROS_INFO("UP->DOWN");
        //     ctrl_msg.position[0] = 0.5; //(0.5m)
        //     ctrl_msg.velocity[0] = 0.5; //(0.5m/s)
        //     ctrl_msg.position[1] = 0.1; 
        //     ctrl_msg.velocity[1] = 5;    //(5 degree/s)
        //     state = mani_fold;
        // }
        // else if (state == mani_fold){
        //     ROS_INFO("DOWN->ZERO");
        //     ctrl_msg.position[0] = 0; //(0.5m)
        //     ctrl_msg.velocity[0] = 0.5; //(0.5m/s)
        //     ctrl_msg.position[1] = 0.1; 
        //     ctrl_msg.velocity[1] = 5;    //(5 degree/s)
        //     state = mani_zero;
        // }

        // arm_move.publish(ctrl_msg);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
 }
