void handleReceivedUDP(const boost::system::error_code& error,
std::size_t bytes_transferred)
{
if (error == boost::asio::error::operation_aborted)
return; /* we're done */
if (error) {
printf("SC_UdpInPort: received error - %s", error.message().c_str());
startReceiveUDP();
return;
}
if (mWorld->mDumpOSC) dumpOSC(mWorld->mDumpOSC, bytes_transferred, recvBuffer.data());
OSC_Packet * packet = (OSC_Packet*)malloc(sizeof(OSC_Packet));
packet->mReplyAddr.mProtocol = kUDP;
packet->mReplyAddr.mAddress = remoteEndpoint.address();
packet->mReplyAddr.mPort = remoteEndpoint.port();
packet->mReplyAddr.mSocket = udpSocket.native_handle();
packet->mReplyAddr.mReplyFunc = udp_reply_func;
packet->mReplyAddr.mReplyData = (void*)&udpSocket;
packet->mSize = bytes_transferred;
if (!UnrollOSCPacket(mWorld, bytes_transferred, recvBuffer.data(), packet))
free(packet);
startReceiveUDP();
}
void end_read(const boost::system::error_code &ec, std::size_t bytes_transferred)
{
if (!ec)
{
std::cout << std::string(buffer.data(), bytes_transferred) << std::flush;
begin_read();
}
}
inline const_buffer_container_1 buffer(const boost::array<Pod_Type, N>& data,
std::size_t max_size_in_bytes)
{
return const_buffer_container_1(
const_buffer(data.data(),
data.size() * sizeof(Pod_Type) < max_size_in_bytes
? data.size() * sizeof(Pod_Type) : max_size_in_bytes));
}
// 请留意，read_handler() 在将数据写出至 std::cout 之后，会再次调用 async_read_some() 方法。
// 这是必需的，因为无法保证仅在一次异步操作中就可以接收到整个网页。
void read_handler(const boost::system::error_code &ec, std::size_t bytes_transferred)
{
if (!ec)
{
std::cout << std::string(buffer.data(), bytes_transferred) << std::endl;
sock.async_read_some(boost::asio::buffer(buffer), read_handler); //!! 一直读，直到EOF
}
}
void read_handler(const boost::system::error_code& _ec, std::size_t _bytes_transferred)
{
if(!_ec)
{
std::cout << std::string(g_buffer.data(), _bytes_transferred) << std::endl;
g_socket.async_read_some(boost::asio::buffer(g_buffer), read_handler);
//boost::asio::write(g_socket, boost::asio::buffer("ready"));
if(std::string(g_buffer.data()) == "ready")
{
std::cout << " > disconnect client" << std::endl;
boost::system::error_code error;
g_socket.shutdown(boost::asio::ip::tcp::socket::shutdown_both, error);
g_socket.close();
}
}
else
std::cerr << " > could not read" << std::endl;
}
inline void connection::handle_read(const boost::system::error_code& e,
std::size_t bytes_transferred) {
if (!e) {
boost::tribool result;
boost::tie(result, boost::tuples::ignore) = request_parser_.parse(
*request_, buffer_.data(), buffer_.data() + bytes_transferred);
if (result) {
try {
request_handler_.handle_request(*request_, reply_);
} catch (webserver::reply rep) {
reply_ = rep;
}
std::cerr << reply_.content << std::endl;
boost::asio::async_write(socket_, reply_.to_buffers(),
strand_.wrap(
boost::bind(&connection::handle_write, shared_from_this(),
boost::asio::placeholders::error)));
} else if (!result) {
reply_ = reply::stock_reply(reply::bad_request);
boost::asio::async_write(socket_, reply_.to_buffers(),
strand_.wrap(
boost::bind(&connection::handle_write, shared_from_this(),
boost::asio::placeholders::error)));
} else {
socket_.async_read_some(boost::asio::buffer(buffer_),
strand_.wrap(
boost::bind(&connection::handle_read, shared_from_this(),
boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred)));
}
}
// If an error occurs then no new asynchronous operations are started. This
// means that all shared_ptr references to the connection object will
// disappear and the object will be destroyed automatically after this
// handler returns. The connection class's destructor closes the socket.
}
void put_string(entry& e, boost::array<char, 64>& sig, boost::uint64_t& seq
, std::string const& salt, char const* public_key, char const* private_key
, char const* str)
{
using libtorrent::dht::sign_mutable_item;
e = std::string(str);
std::vector<char> buf;
bencode(std::back_inserter(buf), e);
++seq;
sign_mutable_item(std::pair<char const*, int>(&buf[0], buf.size())
, std::pair<char const*, int>(&salt[0], salt.size())
, seq
, public_key
, private_key
, sig.data());
}
void ReceiveHandle(const boost::system::error_code e, const std::size_t size)
{
StartReceive();
if (e)
{
std::cout << e.value() << " " << e.message() << std::endl;
return;
}
const std::string data(recv_buf.data(), size);
std::cout << "Received: " << data << std::endl;
std::vector<std::string> args;
boost::algorithm::split(args, data, boost::algorithm::is_space());
if (args.size() > 1)
{
boost::asio::ip::udp::resolver resolver(io_service);
boost::asio::ip::udp::resolver::query query(boost::asio::ip::udp::v4(), args[0], args[1]);
boost::asio::ip::udp::endpoint clientEp = *resolver.resolve(query);
const std::string hello("ping!");
g_pSocket->send_to(boost::asio::buffer(hello), clientEp);
const std::string epStr = clientEp.address().to_string() + ":" + conv::cast<std::string>(clientEp.port());
std::cout << "Sending ping! to: " << epStr << std::endl;
}
else
{
const std::string hello("pong!");
g_pSocket->send_to(boost::asio::buffer(hello), ep);
const std::string epStr = ep.address().to_string() + ":" + conv::cast<std::string>(ep.port());
std::cout << "Sending pong! to: " << epStr << std::endl;
}
}
void handle_read(const boost::system::error_code& error,
size_t bytes_transferred)
{
if (!error)
{
boost::asio::async_write(socket_,
boost::asio::buffer(data_.data(), bytes_transferred),
boost::bind(&session::handle_write, shared_from_this(),
boost::asio::placeholders::error));
}
else
{
std::cerr << esc(MAKE_RED)
<< LOG_HEADER
<< "Unable to receive data: "
<< error.message()
<< " [Errno: "
<< error.value()
<< ']'
<< esc(RESET_COLOR)
<< std::endl;
}
}
inline V evaluate_polynomial(const boost::array<T,N>& a, const V& val)
{
typedef mpl::int_<N> tag_type;
return detail::evaluate_polynomial_c_imp(static_cast<const T*>(a.data()), val, static_cast<tag_type const*>(0));
}
inline V evaluate_rational(const boost::array<T,N>& a, const boost::array<U,N>& b, const V& z)
{
return detail::evaluate_rational_c_imp(a.data(), b.data(), z, static_cast<mpl::int_<N>*>(0));
}
array_view(boost::array<mutable_value_type, N> const &array)
: Length(Length::template literal<N>())
, m_data(array.data())
{
}
void read_handler(boost::system::error_code ec, std::size_t bytes_transferred)
{
if (!ec) {
ROS_INFO("Received %d bytes", (int)bytes_transferred);
lunabotics::proto::Telecommand tc;
if (!tc.ParseFromArray(buffer.data(), bytes_transferred)) {
ROS_ERROR("Failed to parse Telecommand object");
emergency_stop();
return;
}
lunabotics::Connection connectionMsg;
connectionMsg.port = tc.reply_port();
connectionMsg.ip = sock.remote_endpoint().address().to_string();
connPublisher.publish(connectionMsg);
switch(tc.type()) {
case lunabotics::proto::Telecommand::SET_AUTONOMY: {
ROS_INFO("%s autonomy", tc.autonomy_data().enabled() ? "Enabling" : "Disabling");
std_msgs::Bool autonomyMsg;
autonomyMsg.data = tc.autonomy_data().enabled();
autonomyPublisher.publish(autonomyMsg);
}
break;
case lunabotics::proto::Telecommand::STEERING_MODE: {
lunabotics::proto::SteeringModeType type = tc.steering_mode_data().type();
ROS_INFO("Switching control mode to %s", steeringModeToString(type).c_str());
lunabotics::SteeringMode controlModeMsg;
controlModeMsg.mode = type;
controlModeMsg.distance_accuracy = tc.steering_mode_data().position_accuracy();
controlModeMsg.angle_accuracy = tc.steering_mode_data().heading_accuracy();
controlModeMsg.linear_speed_limit = tc.steering_mode_data().max_linear_velocity();
controlModeMsg.bezier_segments = tc.steering_mode_data().bezier_curve_segments();
controlModePublisher.publish(controlModeMsg);
}
break;
case lunabotics::proto::Telecommand::TELEOPERATION: {
lunabotics::Teleoperation teleopMsg;
teleopMsg.forward = tc.teleoperation_data().forward();
teleopMsg.backward = tc.teleoperation_data().backward();
teleopMsg.left = tc.teleoperation_data().left();
teleopMsg.right = tc.teleoperation_data().right();
teleoperationPublisher.publish(teleopMsg);
}
break;
case lunabotics::proto::Telecommand::DEFINE_ROUTE: {
lunabotics::Goal goalMsg;
const lunabotics::proto::Telecommand::DefineRoute route = tc.define_route_data();
for (int i = 0; i < route.waypoints_size(); i++) {
const lunabotics::proto::Point waypoint = route.waypoints(i);
geometry_msgs::Point pt = lunabotics::geometry_msgs_Point_from_Point(lunabotics::CreatePoint(waypoint.x(), waypoint.y()));
goalMsg.waypoints.push_back(pt);
ROS_INFO("Navigation to (%.1f,%.1f)", pt.x, pt.y);
}
goalPublisher.publish(goalMsg);
}
break;
case lunabotics::proto::Telecommand::REQUEST_MAP: {
ROS_INFO("Receiving map request");
std_msgs::Empty emptyMsg;
mapRequestPublisher.publish(emptyMsg);
}
break;
case lunabotics::proto::Telecommand::ADJUST_PID: {
lunabotics::PID pidMsg;
pidMsg.p = tc.adjust_pid_data().p();
pidMsg.i = tc.adjust_pid_data().i();
pidMsg.d = tc.adjust_pid_data().d();
pidMsg.feedback_min_offset = tc.adjust_pid_data().feedback_min_offset();
pidMsg.feedback_multiplier = tc.adjust_pid_data().feedback_multiplier();
pidMsg.feedforward_min_offset = tc.adjust_pid_data().feedforward_min_offset();
pidMsg.feedforward_fraction = tc.adjust_pid_data().feedforward_fraction();
pidPublisher.publish(pidMsg);
}
break;
case lunabotics::proto::Telecommand::ADJUST_WHEELS: {
switch (tc.all_wheel_control_data().all_wheel_type()) {
case lunabotics::proto::AllWheelControl::EXPLICIT: {
lunabotics::proto::AllWheelState::Wheels driving = tc.all_wheel_control_data().explicit_data().driving();
lunabotics::proto::AllWheelState::Wheels steering = tc.all_wheel_control_data().explicit_data().steering();
float left_front = steering.left_front();
float right_front = steering.right_front();
float left_rear = steering.left_rear();
float right_rear = steering.right_rear();
lunabotics::validateAngles(left_front, right_front, left_rear, right_rear);
lunabotics::AllWheelState msg;
msg.driving.left_front = driving.left_front();
msg.driving.right_front = driving.right_front();
msg.driving.left_rear = driving.left_rear();
msg.driving.right_rear = driving.right_rear();
msg.steering.left_front = left_front;
msg.steering.right_front = right_front;
msg.steering.left_rear = left_rear;
msg.steering.right_rear = right_rear;
allWheelPublisher.publish(msg);
}
break;
case lunabotics::proto::AllWheelControl::PREDEFINED: {
lunabotics::AllWheelCommon msg;
msg.predefined_cmd = tc.all_wheel_control_data().predefined_data().command();
msg.wheel_velocity = DEFAULT_WHEEL_VELOCITY;
allWheelCommonPublisher.publish(msg);
}
break;
case lunabotics::proto::AllWheelControl::ICR: {
lunabotics::ICRControl msg;
msg.ICR.x = tc.all_wheel_control_data().icr_data().icr().x();
msg.ICR.y = tc.all_wheel_control_data().icr_data().icr().y();
msg.velocity = tc.all_wheel_control_data().icr_data().velocity();
ICRPublisher.publish(msg);
}
break;
case lunabotics::proto::AllWheelControl::CRAB: {
lunabotics::CrabControl msg;
msg.heading = tc.all_wheel_control_data().crab_data().heading();
msg.velocity = tc.all_wheel_control_data().crab_data().velocity();
CrabPublisher.publish(msg);
}
break;
default:
break;
}
}
break;
default:
break;
}
}
else if (ec.value() == boost::asio::error::eof) {
ROS_INFO("Connection closed by client");
}
else {
ROS_WARN("Failed to read data. Error code %s", ec.message().c_str());
}
sock.shutdown(boost::asio::ip::tcp::socket::shutdown_both, ec);
sock.close();
acceptor.async_accept(sock, accept_handler);
}
/*implicit*/ array_view(boost::array<T, N> const& a) noexcept
: length_(N), data_(a.data())
{}
/**
* set data from boost::array
*/
void set(const boost::array<float, 9ul> d) {
set(static_cast<const float*>(d.data()));
}
uint64_t handler::parse_request(size_t size) {
return parse(m_buffer.data(), size);
}
inline const_buffer_container_1 buffer(const boost::array<Pod_Type, N>& data)
{
return const_buffer_container_1(
const_buffer(data.data(), data.size() * sizeof(Pod_Type)));
}
inline V evaluate_odd_polynomial(const boost::array<T,N>& a, const V& z)
{
typedef mpl::int_<N-1> tag_type;
return a[0] + z * detail::evaluate_polynomial_c_imp(static_cast<const T*>(a.data()) + 1, V(z*z), static_cast<tag_type const*>(0));
}
T* data() {
return buf.data();
}
const T* data() const {
return buf.data();
}