前面说过，机器人和机器人后端管理系统之间通常要求频繁双向传输数据、尤其需要最好保持长连接的通讯需求，使用WebSocket连接是最好的选择，可以基于RosBridge来实现，但是RosBridge实现的是websocket server，对于要求机器人后端管理系统在系统启动时主动连接机器人的需求是比较合适的，对于反过来要求机器人启动时作为client主动连接机器人管理系统的这种更常见需求，RosBridge不能满足要求，一个快捷实现办法就是安装python版的websocket-client，然后基于python版websocket-client进行python编程，但是我感觉对于有复杂类型的交互数据需要传输和处理并且需要性能尽可能快的话，当然还是自己使用C++来实现一个ROS通讯节点作为与机器人后端通讯的websocket client比较好。
自己用C++实现websocket那些协议内容显然要费不少时间去编写和调试，对于项目要求进度尽可能快的情况下不大现实，于是网上找了评价不错的C++实现的开源websocket框架websocketpp来基于它开发，仔细看了它的代码，找到一个比较合适的client示例代码，在它基础上做开发比较合适：
#include <websocketpp/config/asio_no_tls_client.hpp>
#include <websocketpp/client.hpp>
#include <websocketpp/common/thread.hpp>
#include <websocketpp/common/memory.hpp>
#include <cstdlib>
#include <iostream>
#include <map>
#include <string>
#include <sstream>
typedef websocketpp::client<websocketpp::config::asio_client> client;
class connection_metadata {
public:
typedef websocketpp::lib::shared_ptr<connection_metadata> ptr;
connection_metadata(int id, websocketpp::connection_hdl hdl, std::string uri)
: m_id(id)
, m_hdl(hdl)
, m_status("Connecting")
, m_uri(uri)
, m_server("N/A")
{}
void on_open(client * c, websocketpp::connection_hdl hdl) {
m_status = "Open";
client::connection_ptr con = c->get_con_from_hdl(hdl);
m_server = con->get_response_header("Server");
}
void on_fail(client * c, websocketpp::connection_hdl hdl) {
m_status = "Failed";
client::connection_ptr con = c->get_con_from_hdl(hdl);
m_server = con->get_response_header("Server");
m_error_reason = con->get_ec().message();
}
void on_close(client * c, websocketpp::connection_hdl hdl) {
m_status = "Closed";
client::connection_ptr con = c->get_con_from_hdl(hdl);
std::stringstream s;
s << "close code: " << con->get_remote_close_code() << " ("
<< websocketpp::close::status::get_string(con->get_remote_close_code())
<< "), close reason: " << con->get_remote_close_reason();
m_error_reason = s.str();
}
void on_message(websocketpp::connection_hdl, client::message_ptr msg) {
if (msg->get_opcode() == websocketpp::frame::opcode::text) {
m_messages.push_back("<< " + msg->get_payload());
} else {
m_messages.push_back("<< " + websocketpp::utility::to_hex(msg->get_payload()));
}
}
websocketpp::connection_hdl get_hdl() const {
return m_hdl;
}
int get_id() const {
return m_id;
}
std::string get_status() const {
return m_status;
}
void record_sent_message(std::string message) {
m_messages.push_back(">> " + message);
}
friend std::ostream & operator<< (std::ostream & out, connection_metadata const & data);
private:
int m_id;
websocketpp::connection_hdl m_hdl;
std::string m_status;
std::string m_uri;
std::string m_server;
std::string m_error_reason;
std::vector<std::string> m_messages;
};
std::ostream & operator<< (std::ostream & out, connection_metadata const & data) {
out << "> URI: " << data.m_uri << "\n"
<< "> Status: " << data.m_status << "\n"
<< "> Remote Server: " << (data.m_server.empty() ? "None Specified" : data.m_server) << "\n"
<< "> Error/close reason: " << (data.m_error_reason.empty() ? "N/A" : data.m_error_reason) << "\n";
out << "> Messages Processed: (" << data.m_messages.size() << ") \n";
std::vector<std::string>::const_iterator it;
for (it = data.m_messages.begin(); it != data.m_messages.end(); ++it) {
out << *it << "\n";
}
return out;
}
class websocket_endpoint {
public:
websocket_endpoint () : m_next_id(0) {
m_endpoint.clear_access_channels(websocketpp::log::alevel::all);
m_endpoint.clear_error_channels(websocketpp::log::elevel::all);
m_endpoint.init_asio();
m_endpoint.start_perpetual();
m_thread = websocketpp::lib::make_shared<websocketpp::lib::thread>(&client::run, &m_endpoint);
}
~websocket_endpoint() {
m_endpoint.stop_perpetual();
for (con_list::const_iterator it = m_connection_list.begin(); it != m_connection_list.end(); ++it) {
if (it->second->get_status() != "Open") {
// Only close open connections
continue;
}
std::cout << "> Closing connection " << it->second->get_id() << std::endl;
websocketpp::lib::error_code ec;
m_endpoint.close(it->second->get_hdl(), websocketpp::close::status::going_away, "", ec);
if (ec) {
std::cout << "> Error closing connection " << it->second->get_id() << ": "
<< ec.message() << std::endl;
}
}
m_thread->join();
}
int connect(std::string const & uri) {
websocketpp::lib::error_code ec;
client::connection_ptr con = m_endpoint.get_connection(uri, ec);
if (ec) {
std::cout << "> Connect initialization error: " << ec.message() << std::endl;
return -1;
}
int new_id = m_next_id++;
connection_metadata::ptr metadata_ptr = websocketpp::lib::make_shared<connection_metadata>(new_id, con->get_handle(), uri);
m_connection_list[new_id] = metadata_ptr;
con->set_open_handler(websocketpp::lib::bind(
&connection_metadata::on_open,
metadata_ptr,
&m_endpoint,
websocketpp::lib::placeholders::_1
));
con->set_fail_handler(websocketpp::lib::bind(
&connection_metadata::on_fail,
metadata_ptr,
&m_endpoint,
websocketpp::lib::placeholders::_1
));
con->set_close_handler(websocketpp::lib::bind(
&connection_metadata::on_close,
metadata_ptr,
&m_endpoint,
websocketpp::lib::placeholders::_1
));
con->set_message_handler(websocketpp::lib::bind(
&connection_metadata::on_message,
metadata_ptr,
websocketpp::lib::placeholders::_1,
websocketpp::lib::placeholders::_2
));
m_endpoint.connect(con);
return new_id;
}
void close(int id, websocketpp::close::status::value code, std::string reason) {
websocketpp::lib::error_code ec;
con_list::iterator metadata_it = m_connection_list.find(id);
if (metadata_it == m_connection_list.end()) {
std::cout << "> No connection found with id " << id << std::endl;
return;
}
m_endpoint.close(metadata_it->second->get_hdl(), code, reason, ec);
if (ec) {
std::cout << "> Error initiating close: " << ec.message() << std::endl;
}
}
void send(int id, std::string message) {
websocketpp::lib::error_code ec;
con_list::iterator metadata_it = m_connection_list.find(id);
if (metadata_it == m_connection_list.end()) {
std::cout << "> No connection found with id " << id << std::endl;
return;
}
m_endpoint.send(metadata_it->second->get_hdl(), message, websocketpp::frame::opcode::text, ec);
if (ec) {
std::cout << "> Error sending message: " << ec.message() << std::endl;
return;
}
metadata_it->second->record_sent_message(message);
}
connection_metadata::ptr get_metadata(int id) const {
con_list::const_iterator metadata_it = m_connection_list.find(id);
if (metadata_it == m_connection_list.end()) {
return connection_metadata::ptr();
} else {
return metadata_it->second;
}
}
private:
typedef std::map<int,connection_metadata::ptr> con_list;
client m_endpoint;
websocketpp::lib::shared_ptr<websocketpp::lib::thread> m_thread;
con_list m_connection_list;
int m_next_id;
};
int main() {
bool done = false;
std::string input;
websocket_endpoint endpoint;
while (!done) {
std::cout << "Enter Command: ";
std::getline(std::cin, input);
if (input == "quit") {
done = true;
} else if (input == "help") {
std::cout
<< "\nCommand List:\n"
<< "connect <ws uri>\n"
<< "send <connection id> <message>\n"
<< "close <connection id> [<close code:default=1000>] [<close reason>]\n"
<< "show <connection id>\n"
<< "help: Display this help text\n"
<< "quit: Exit the program\n"
<< std::endl;
} else if (input.substr(0,7) == "connect") {
int id = endpoint.connect(input.substr(8));
if (id != -1) {
std::cout << "> Created connection with id " << id << std::endl;
}
} else if (input.substr(0,4) == "send") {
std::stringstream ss(input);
std::string cmd;
int id;
std::string message;
ss >> cmd >> id;
std::getline(ss,message);
endpoint.send(id, message);
} else if (input.substr(0,5) == "close") {
std::stringstream ss(input);
std::string cmd;
int id;
int close_code = websocketpp::close::status::normal;
std::string reason;
ss >> cmd >> id >> close_code;
std::getline(ss,reason);
endpoint.close(id, close_code, reason);
} else if (input.substr(0,4) == "show") {
int id = atoi(input.substr(5).c_str());
connection_metadata::ptr metadata = endpoint.get_metadata(id);
if (metadata) {
std::cout << *metadata << std::endl;
} else {
std::cout << "> Unknown connection id " << id << std::endl;
}
} else {
std::cout << "> Unrecognized Command" << std::endl;
}
}
return 0;
}
上面的代码实现了websocket连接的建立和数据的收发，不过经过试用实验发现有以下缺点需要改进：
1) websocket_endpoint::connect()里只是发了连接请求，没有等待连接完成成功就返回，另外对于因为IP或端口错误或网络不通的情况也没做检查和判断；
2)同一连接上的数据的发送和接收之间的对应关系没有管理，对于请求-响应式通讯在发送数据后等待响应数据到来后才做后面的处理的逻辑没有实现。
另外，实现为ROS节点需要增加代码，此节点收到来自机器人后端管理系统发来的数据后，可以解析后通过RosBridge Server遵循rosbridge v2.0 Protocol Specification调用相应的其他ROS节点的service或给其他Ros节点发布消息，也可以对来自机器人后端管理系统发来的数据不做任何解析直接通过Publish/Subscribe机制转发给其他相应节点后再解析处理，这个就看你想怎么设计。
为实现上述目标，我对上面的源码做了修改和增加，具体作用参见代码附近的注释：
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <unistd.h>
#include <websocketpp/config/asio_no_tls_client.hpp>
#include <websocketpp/client.hpp>
#include <websocketpp/common/thread.hpp>
#include <websocketpp/common/memory.hpp>
#include <cstdlib>
#include <iostream>
#include <map>
#include <string>
#include <sstream>
typedef websocketpp::client<websocketpp::config::asio_client> client;
class websocket_endpoint;
class connection_metadata {
public:
typedef websocketpp::lib::shared_ptr<connection_metadata> ptr;
connection_metadata(int id, websocketpp::connection_hdl hdl, std::string uri,websocket_endpoint* endpoint);
void on_open(client * c, websocketpp::connection_hdl hdl) {
m_status = "Open";
client::connection_ptr con = c->get_con_from_hdl(hdl);
m_server = con->get_response_header("Server");
}
void on_fail(client * c, websocketpp::connection_hdl hdl) {
m_status = "Failed";
client::connection_ptr con = c->get_con_from_hdl(hdl);
m_server = con->get_response_header("Server");
m_error_reason = con->get_ec().message();
}
void on_close(client * c, websocketpp::connection_hdl hdl) {
m_status = "Closed";
client::connection_ptr con = c->get_con_from_hdl(hdl);
std::stringstream s;
s << "close code: " << con->get_remote_close_code() << " ("
<< websocketpp::close::status::get_string(con->get_remote_close_code())
<< "), close reason: " << con->get_remote_close_reason();
m_error_reason = s.str();
}
void on_message(websocketpp::connection_hdl, client::message_ptr msg);
websocketpp::connection_hdl get_hdl() const {
return m_hdl;
}
int get_id() const {
return m_id;
}
std::string get_status() const {
return m_status;
}
void record_sent_message(std::string message) {
m_messages.push_back(">> " + message);
}
friend std::ostream & operator<< (std::ostream & out, connection_metadata const & data);
private:
int m_id;
websocketpp::connection_hdl m_hdl;
std::string m_status;
std::string m_uri;
std::string m_server;
std::string m_error_reason;
std::vector<std::string> m_messages;
websocket_endpoint* m_endpoint;
};
class websocket_endpoint {
public:
websocket_endpoint () : m_next_id(0) {
m_endpoint.clear_access_channels(websocketpp::log::alevel::all);
m_endpoint.clear_error_channels(websocketpp::log::elevel::all);
m_endpoint.init_asio();
m_endpoint.start_perpetual();
m_thread = websocketpp::lib::make_shared<websocketpp::lib::thread>(&client::run, &m_endpoint);
}
~websocket_endpoint() {
m_endpoint.stop_perpetual();
for (con_list::const_iterator it = m_connection_list.begin(); it != m_connection_list.end(); ++it) {
if (it->second->get_status() != "Open") {
// Only close open connections
continue;
}
ROS_INFO_STREAM("> Closing connection " << it->second->get_id() );
websocketpp::lib::error_code ec;
m_endpoint.close(it->second->get_hdl(), websocketpp::close::status::going_away, "", ec);
if (ec) {
ROS_ERROR_STREAM("> Error closing connection " << it->second->get_id() << ": "
<< ec.message() );
}
}
m_thread->join();
}
int connect(std::string const & uri,int timeout) { //timeout unit: second
websocketpp::lib::error_code ec;
client::connection_ptr con = m_endpoint.get_connection(uri, ec);
if (ec) {
ROS_ERROR_STREAM("> Connect initialization error: " << ec.message() );
return -1;
}
int new_id = m_next_id++;
connection_metadata::ptr metadata_ptr = websocketpp::lib::make_shared<connection_metadata>(new_id, con->get_handle(), uri,this);
m_connection_list[new_id] = metadata_ptr;
con->set_open_handler(websocketpp::lib::bind(
&connection_metadata::on_open,
metadata_ptr,
&m_endpoint,
websocketpp::lib::placeholders::_1
));
con->set_fail_handler(websocketpp::lib::bind(
&connection_metadata::on_fail,
metadata_ptr,
&m_endpoint,
websocketpp::lib::placeholders::_1
));
con->set_close_handler(websocketpp::lib::bind(
&connection_metadata::on_close,
metadata_ptr,
&m_endpoint,
websocketpp::lib::placeholders::_1
));
con->set_message_handler(websocketpp::lib::bind(
&connection_metadata::on_message,
metadata_ptr,
websocketpp::lib::placeholders::_1,
websocketpp::lib::placeholders::_2
));
m_endpoint.connect(con);
//等待连接完成非常重要！！！
int count=0;
while(metadata_ptr->get_status() != "Open" && ++count <= timeout*10 ){
usleep(100000);
}
m_conn_id = new_id;
if(metadata_ptr->get_status() != "Open"){ //连接超时！
ROS_INFO_STREAM("Timeout! failed to connect to "<<uri<<" in "<<timeout<<" seconds!");
m_conn_id = -1;
return -1;
}
else
return new_id;
}
void close(int id, websocketpp::close::status::value code, std::string reason) {
websocketpp::lib::error_code ec;
con_list::iterator metadata_it = m_connection_list.find(id);
if (metadata_it == m_connection_list.end()) {
ROS_INFO_STREAM("> No connection found with id " << id );
return;
}
m_endpoint.close(metadata_it->second->get_hdl(), code, reason, ec);
if (ec) {
ROS_ERROR_STREAM( "> Error initiating close: " << ec.message() );
}
}
void send(int id, std::string message) {
websocketpp::lib::error_code ec;
con_list::iterator metadata_it = m_connection_list.find(id);
if (metadata_it == m_connection_list.end()) {
ROS_INFO_STREAM("> No connection found with id " << id );
return;
}
m_endpoint.send(metadata_it->second->get_hdl(), message, websocketpp::frame::opcode::text, ec);
if (ec) {
ROS_ERROR_STREAM("> Error sending message: " << ec.message());
return;
}
m_sendRecvMap[id]=false;
//metadata_it->second->record_sent_message(message);
}
connection_metadata::ptr get_metadata(int id) const {
con_list::const_iterator metadata_it = m_connection_list.find(id);
if (metadata_it == m_connection_list.end()) {
return connection_metadata::ptr();
} else {
return metadata_it->second;
}
}
std::map<int,bool> getSendRecvMap(){
return m_sendRecvMap;
}
void setPublisher(ros::Publisher pub) {
m_pub = pub;
}
//MC -> CCP
void MC2CCPCallback(const std_msgs::String::ConstPtr& msg)
{
std::string str = msg->data;
ROS_INFO_STREAM("[MC2CCPCallback]msg received: " << str <<", send to connection:"<<m_conn_id);
send(m_conn_id,str);
}
//CCP -> MC
void handleTextMessage(int id,std::string message) {
ROS_INFO_STREAM( "[handleTextMessage]" << message );
//publish CCP's msg to MC, connection id is reserved for future use.
std_msgs::String str;
str.data = message.c_str();
m_pub.publish(str);
m_sendRecvMap[id]=true;
}
void handleBinaryMessage(int id,std::string message) {
}
private:
typedef std::map<int,connection_metadata::ptr> con_list;
client m_endpoint;
websocketpp::lib::shared_ptr<websocketpp::lib::thread> m_thread;
con_list m_connection_list;
int m_next_id;
int m_conn_id=-1;
std::map<int,bool> m_sendRecvMap;
ros::Publisher m_pub;
};
std::ostream & operator<< (std::ostream & out, connection_metadata const & data) {
out << "> URI: " << data.m_uri << "\n"
<< "> Status: " << data.m_status << "\n"
<< "> Remote Server: " << (data.m_server.empty() ? "None Specified" : data.m_server) << "\n"
<< "> Error/close reason: " << (data.m_error_reason.empty() ? "N/A" : data.m_error_reason) << "\n";
out << "> Messages Processed: (" << data.m_messages.size() << ") \n";
std::vector<std::string>::const_iterator it;
for (it = data.m_messages.begin(); it != data.m_messages.end(); ++it) {
out << *it << "\n";
}
return out;
}
connection_metadata::connection_metadata(int id, websocketpp::connection_hdl hdl, std::string uri,websocket_endpoint* endpoint)
: m_id(id)
, m_hdl(hdl)
, m_status("Connecting")
, m_uri(uri)
, m_server("N/A"),
m_endpoint(endpoint)
{}
void connection_metadata::on_message(websocketpp::connection_hdl, client::message_ptr msg) {
if (msg->get_opcode() == websocketpp::frame::opcode::text) {
//m_messages.push_back("<< " + msg->get_payload());
m_endpoint->handleTextMessage(m_id,msg->get_payload());
} else {
//m_messages.push_back("<< " + websocketpp::utility::to_hex(msg->get_payload()));
m_endpoint->handleTextMessage(m_id,websocketpp::utility::to_hex(msg->get_payload()));
}
}
bool bShutDown = false;
void shutdownCallback(const std_msgs::StringConstPtr& msg)
{
std::string str = msg->data;
if (str=="MC_CMD_SHUTDOWN")
bShutDown = true;
}
int main(int argc, char ** argv) {
std::string input;
// 初始化节点
ros::init(argc, argv, "ws_server");
ros::NodeHandle node;
ros::Rate rate(100.0);
ros::Subscriber sub_shutdown = node.subscribe("ws_server/MC2WSS", 10, &shutdownCallback);
//目前只支持创建一个到CCP的websocket连接
websocket_endpoint endpoint;
//CCP -> Robot's ws_server -> Robot's MC
ros::Publisher pub =
node.advertise<std_msgs::String>("ws_server/CCP2MC", 10);
endpoint.setPublisher(pub);
//Robot's MC -> Robot's ws_server -> CCP
ros::Subscriber sub = node.subscribe("ws_server/MC2CCP", 10, &websocket_endpoint::MC2CCPCallback,&endpoint);
std::string test_mode;
int timeout;
node.getParam("/ws_server/test_mode", test_mode);
node.getParam("/ws_server/timeout", timeout);
//ROS_INFO_STREAM("test_mode="<< test_mode);
if (test_mode == "yes") { // yes/no
std::vector<int> ids;
while (!bShutDown) {
ROS_INFO_STREAM( "Enter Command: ");
std::getline(std::cin, input);
if (input == "quit") {
bShutDown = true;
int id;
int close_code = websocketpp::close::status::normal;
std::string reason="quit";
std::vector<int>::const_iterator it;
for (it = ids.begin(); it != ids.end(); ++it) {
id = *it;
// endpoint.close(id, close_code, reason);
}
} else if (input == "help") {
ROS_INFO_STREAM("\nCommand List:\n" << "connect <ws uri>\n"
<< "send <connection id> <message>\n"
<< "close <connection id> [<close code:default=1000>] [<close reason>]\n"
<< "show <connection id>\n"
<< "help: Display this help text\n"
<< "quit: Exit the program\n" );
} else if (input.substr(0, 7) == "connect") {
int id = endpoint.connect(input.substr(8),timeout);
if (id != -1) {
ids.push_back(id);
ROS_INFO_STREAM( "> Created connection with id " << id);
}
} else if (input.substr(0, 4) == "send") {
std::stringstream ss(input);
std::string cmd;
int id;
std::string message;
ss >> cmd >> id;
std::getline(ss, message);
endpoint.send(id, message);
} else if (input.substr(0, 5) == "close") {
std::stringstream ss(input);
std::string cmd;
int id;
int close_code = websocketpp::close::status::normal;
std::string reason;
ss >> cmd >> id >> close_code;
std::getline(ss, reason);
endpoint.close(id, close_code, reason);
} else if (input.substr(0, 4) == "show") {
int id = atoi(input.substr(5).c_str());
connection_metadata::ptr metadata = endpoint.get_metadata(id);
if (metadata) {
std::cout << *metadata << std::endl;
} else {
ROS_INFO_STREAM("> Unknown connection id " << id );
}
} else {
ROS_INFO_STREAM("> Unrecognized Command" );
}
}
} else {
std::string protocol;
std::string ip;
int port;
std::string entry;
std::string uri;
std::stringstream ss;
node.getParam("/ws_server/protocol", protocol);
node.getParam("/ws_server/ip", ip);
node.getParam("/ws_server/port", port);
node.getParam("/ws_server/entry", entry);
ss<<port;
uri = protocol + "://" + ip + ":" + ss.str() + entry;
//目前只Robot到CCP创建支持一个websocket连接，如果Robot需要同时创建多个websocket连接连接到不同的控制系统，两种改造办法:
//1.创建多个websocket_endpoint实例，分别用于不同的连接，这种办法最简单；
//2.只创建一个websocket_endpoint实例，调用多次connect()连接到不同到系统并记录connect()返回的connection id，
// 收发数据时都得根据connection id识别不同的连接，另外pub和sub都得改造:
// publish时需要把connection id传给MC并记录，subscriber的callback()也要把MC传来的connection id传给send()以发送数据给对应的CP
int id = endpoint.connect(uri,timeout);
ROS_INFO_STREAM("connect "<< uri <<",id="<<id );
if(id<0)
return -1;
//Shutdown procedure:
//1. CCP sends a shutdown cmd to MC via ws_server by websocket and publishing
//2. MC does something before shutting down,and sends a shutdown message to ws_server by publishing.
//3. ws_server exits the while-loop and shuts down
while(node.ok() && !bShutDown ){
rate.sleep(); //rate is 100 HZ, so this sleep time is equal to or less than 10 miliseconds
ros::spinOnce();
//ROS_INFO_STREAM("while-loop");
}
#if 1
/*
//Robot notifies CCP of that it is shutting down.
std::string robot_id = "J1001";
std::string message="Robot "+robot_id+" shut down!"; //this is only an example,actually this message should be issued by MC.
endpoint.send(id, message); //send shutdown msg back to CCP
*/
connection_metadata::ptr metadata = endpoint.get_metadata(id);
if (metadata) {
std::cout << *metadata << std::endl;
ROS_INFO_STREAM( "=====shut down======" );
}
int close_code = websocketpp::close::status::normal; //1000
std::string reason="normal exit";
std::map<int,bool> sendRecvMap = endpoint.getSendRecvMap();
std::map<int,bool>::iterator it = sendRecvMap.find(id);
int count=0;
if(it!=sendRecvMap.end()) {
while(!it->second && ++count <= timeout*10 ){
usleep(100000);
}
}
endpoint.close(id,close_code,reason);
#endif
}
//ROS_INFO_STREAM("shut down...");
ros::shutdown();
return 0;
}
上面 CCP表示机器人后端管理系统，MC表示机器人本体的中控系统，上面实现的这个ROS节点就是在CCP和MC之间起到桥梁作用，对CCP和MC之间的通讯数据没做任何解析处理，只负责转发。
原始代码中需要通过命令行输入来测试websocket连接的建立/关闭和数据的收发，我对其予以保留，考虑到正式运行时不需要这个命令行输入测试的功能，在ws_server.launch文件里增加了test_mode来控制，想测试用命令行测试websocket连接和数据收发的话，把test_mode设置为yes即可，正式运行前把test_mode设置为no即可，正式运行模型下，先配置并启动一个websocket server(比如基于Tomcat配置并运行一个能简单基于websocket收发数据并打印出数据的测试web app)来模拟CCP，执行ws_server.launch启动节点连接到此websocket server后，可以手工执行 rostopic pub -1 /ws_server/MC2CCP std_msgs/String "test data" 之类命令，即可看到web app收到并打印出"test data"，如果web app在收到数据后并立即原样发回数据，执行rostopic echo /ws_server/CCP2MC即可看到web app发来的数据。
