11452150858

Committed 22 Oct 2024 02:32AM UTC coverage: 37.403% (+0.02%) from 37.381%

Build # 11452150858

Build Type

push

github

Committed by

web-flow

Commit Message

camera_server: prevent double ack+message (#2430)

It turns out we were sending the ack and message for storage information
as well as capture status twice, once directly in the request handler
callback, and once the MAVSDK user would call the respond function.

We should only call it in the respond function, not in the callback.

Run Details

11105 of 29690 relevant lines covered (37.4%)

255.6 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

0.0

/src/mavsdk/core/tcp_connection.cpp

#include "tcp_connection.h"
#include "log.h"

#ifdef WINDOWS
#ifndef MINGW
#pragma comment(lib, "Ws2_32.lib") // Without this, Ws2_32.lib is not included in static library.
#endif
#else
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <errno.h>
#include <netdb.h>
#endif

#include <cassert>
#include <utility>

#ifndef WINDOWS
#define GET_ERROR(_x) strerror(_x)
#else
#define GET_ERROR(_x) WSAGetLastError()
#endif

namespace mavsdk {

/* change to remote_ip and remote_port */
TcpConnection::TcpConnection(
    Connection::ReceiverCallback receiver_callback,
    std::string remote_ip,
    int remote_port,
    ForwardingOption forwarding_option) :
    Connection(std::move(receiver_callback), forwarding_option),
    _remote_ip(std::move(remote_ip)),
    _remote_port_number(remote_port),
    _should_exit(false)
{}

TcpConnection::~TcpConnection()
{
    // If no one explicitly called stop before, we should at least do it.
    stop();
}

ConnectionResult TcpConnection::start()
{
    if (!start_mavlink_receiver()) {
        return ConnectionResult::ConnectionsExhausted;
    }

    ConnectionResult ret = setup_port();
    if (ret != ConnectionResult::Success) {
        return ret;
    }

    start_recv_thread();

    return ConnectionResult::Success;
}

ConnectionResult TcpConnection::setup_port()
{
#ifdef WINDOWS
    WSADATA wsa;
    if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) {
        LogErr() << "Error: Winsock failed, error: %d", WSAGetLastError();
        _is_ok = false;
        return ConnectionResult::SocketError;
    }
#endif

    _socket_fd.reset(socket(AF_INET, SOCK_STREAM, 0));

    if (_socket_fd.empty()) {
        LogErr() << "socket error" << GET_ERROR(errno);
        _is_ok = false;
        return ConnectionResult::SocketError;
    }

    struct sockaddr_in remote_addr {};
    remote_addr.sin_family = AF_INET;
    remote_addr.sin_port = htons(_remote_port_number);

    struct hostent* hp;
    hp = gethostbyname(_remote_ip.c_str());
    if (hp == nullptr) {
        LogErr() << "Could not get host by name";
        _is_ok = false;
        return ConnectionResult::SocketConnectionError;
    }

    memcpy(&remote_addr.sin_addr, hp->h_addr, hp->h_length);

    if (connect(
            _socket_fd.get(),
            reinterpret_cast<sockaddr*>(&remote_addr),
            sizeof(struct sockaddr_in)) < 0) {
        LogErr() << "connect error: " << GET_ERROR(errno);
        _is_ok = false;
        return ConnectionResult::SocketConnectionError;
    }

    _is_ok = true;
    return ConnectionResult::Success;
}

void TcpConnection::start_recv_thread()
{
    _recv_thread = std::make_unique<std::thread>(&TcpConnection::receive, this);
}

ConnectionResult TcpConnection::stop()
{
    _should_exit = true;

    _socket_fd.close();

    if (_recv_thread) {
        _recv_thread->join();
        _recv_thread.reset();
    }

    // We need to stop this after stopping the receive thread, otherwise
    // it can happen that we interfere with the parsing of a message.
    stop_mavlink_receiver();

    return ConnectionResult::Success;
}

bool TcpConnection::send_message(const mavlink_message_t& message)
{
    if (!_is_ok) {
        return false;
    }

    if (_remote_ip.empty()) {
        LogErr() << "Remote IP unknown";
        return false;
    }

    if (_remote_port_number == 0) {
        LogErr() << "Remote port unknown";
        return false;
    }

    struct sockaddr_in dest_addr {};
    dest_addr.sin_family = AF_INET;

    inet_pton(AF_INET, _remote_ip.c_str(), &dest_addr.sin_addr.s_addr);

    dest_addr.sin_port = htons(_remote_port_number);

    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);

    // TODO: remove this assert again
    assert(buffer_len <= MAVLINK_MAX_PACKET_LEN);

#if !defined(MSG_NOSIGNAL)
    auto flags = 0;
#else
    auto flags = MSG_NOSIGNAL;
#endif

    const auto send_len = sendto(
        _socket_fd.get(),
        reinterpret_cast<char*>(buffer),
        buffer_len,
        flags,
        reinterpret_cast<const sockaddr*>(&dest_addr),
        sizeof(dest_addr));

    if (send_len != buffer_len) {
        LogErr() << "sendto failure: " << GET_ERROR(errno);
        _is_ok = false;
        return false;
    }
    return true;
}

void TcpConnection::receive()
{
    // Enough for MTU 1500 bytes.
    char buffer[2048];

    while (!_should_exit) {
        if (!_is_ok) {
            LogErr() << "TCP receive error, trying to reconnect...";
            std::this_thread::sleep_for(std::chrono::seconds(1));
            setup_port();
        }

        const auto recv_len = recv(_socket_fd.get(), buffer, sizeof(buffer), 0);

        if (recv_len == 0) {
            // This can happen when shutdown is called on the socket,
            // therefore we check _should_exit again.
            _is_ok = false;
            continue;
        }

        if (recv_len < 0) {
            // This happens on destruction when close(_socket_fd.get()) is called,
            // therefore be quiet.
            // LogErr() << "recvfrom error: " << GET_ERROR(errno);
            // Something went wrong, we should try to re-connect in next iteration.
            _is_ok = false;
            continue;
        }

        _mavlink_receiver->set_new_datagram(buffer, static_cast<int>(recv_len));

        // Parse all mavlink messages in one data packet. Once exhausted, we'll exit while.
        while (_mavlink_receiver->parse_message()) {
            receive_message(_mavlink_receiver->get_last_message(), this);
        }
    }
}

} // namespace mavsdk

1	#include "tcp_connection.h"
2	#include "log.h"
3
4	#ifdef WINDOWS
5	#ifndef MINGW
6	#pragma comment(lib, "Ws2_32.lib") // Without this, Ws2_32.lib is not included in static library.
7	#endif
8	#else
9	#include <netinet/in.h>
10	#include <sys/socket.h>
11	#include <arpa/inet.h>
12	#include <errno.h>
13	#include <netdb.h>
14	#endif
15
16	#include <cassert>
17	#include <utility>
18
19	#ifndef WINDOWS
20	#define GET_ERROR(_x) strerror(_x)
21	#else
22	#define GET_ERROR(_x) WSAGetLastError()
23	#endif
24
25	namespace mavsdk {
26
27	/* change to remote_ip and remote_port */
28	TcpConnection::TcpConnection(	×
29	Connection::ReceiverCallback receiver_callback,
30	std::string remote_ip,
31	int remote_port,
32	ForwardingOption forwarding_option) :	×
33	Connection(std::move(receiver_callback), forwarding_option),	×
34	_remote_ip(std::move(remote_ip)),	×
35	_remote_port_number(remote_port),
36	_should_exit(false)	×
37	{}	×
38
39	TcpConnection::~TcpConnection()	×
40	{
41	// If no one explicitly called stop before, we should at least do it.
42	stop();	×
43	}	×
44
45	ConnectionResult TcpConnection::start()	×
46	{
47	if (!start_mavlink_receiver()) {	×
48	return ConnectionResult::ConnectionsExhausted;	×
49	}
50
51	ConnectionResult ret = setup_port();	×
52	if (ret != ConnectionResult::Success) {	×
53	return ret;	×
54	}
55
56	start_recv_thread();	×
57
58	return ConnectionResult::Success;	×
59	}
60
61	ConnectionResult TcpConnection::setup_port()	×
62	{
63	#ifdef WINDOWS
64	WSADATA wsa;
65	if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) {
66	LogErr() << "Error: Winsock failed, error: %d", WSAGetLastError();
67	_is_ok = false;
68	return ConnectionResult::SocketError;
69	}
70	#endif
71
72	_socket_fd.reset(socket(AF_INET, SOCK_STREAM, 0));	×
73
74	if (_socket_fd.empty()) {	×
75	LogErr() << "socket error" << GET_ERROR(errno);	×
76	_is_ok = false;	×
77	return ConnectionResult::SocketError;	×
78	}
79
80	struct sockaddr_in remote_addr {};	×
81	remote_addr.sin_family = AF_INET;	×
82	remote_addr.sin_port = htons(_remote_port_number);	×
83
84	struct hostent* hp;
85	hp = gethostbyname(_remote_ip.c_str());	×
86	if (hp == nullptr) {	×
87	LogErr() << "Could not get host by name";	×
88	_is_ok = false;	×
89	return ConnectionResult::SocketConnectionError;	×
90	}
91
92	memcpy(&remote_addr.sin_addr, hp->h_addr, hp->h_length);	×
93
94	if (connect(	×
95	_socket_fd.get(),
96	reinterpret_cast<sockaddr*>(&remote_addr),
97	sizeof(struct sockaddr_in)) < 0) {	×
98	LogErr() << "connect error: " << GET_ERROR(errno);	×
99	_is_ok = false;	×
100	return ConnectionResult::SocketConnectionError;	×
101	}
102
103	_is_ok = true;	×
104	return ConnectionResult::Success;	×
105	}
106
107	void TcpConnection::start_recv_thread()	×
108	{
109	_recv_thread = std::make_unique<std::thread>(&TcpConnection::receive, this);	×
110	}	×
111
112	ConnectionResult TcpConnection::stop()	×
113	{
114	_should_exit = true;	×
115
116	_socket_fd.close();	×
117
118	if (_recv_thread) {	×
119	_recv_thread->join();	×
120	_recv_thread.reset();	×
121	}
122
123	// We need to stop this after stopping the receive thread, otherwise
124	// it can happen that we interfere with the parsing of a message.
125	stop_mavlink_receiver();	×
126
127	return ConnectionResult::Success;	×
128	}
129
130	bool TcpConnection::send_message(const mavlink_message_t& message)	×
131	{
132	if (!_is_ok) {	×
133	return false;	×
134	}
135
136	if (_remote_ip.empty()) {	×
137	LogErr() << "Remote IP unknown";	×
138	return false;	×
139	}
140
141	if (_remote_port_number == 0) {	×
142	LogErr() << "Remote port unknown";	×
143	return false;	×
144	}
145
146	struct sockaddr_in dest_addr {};	×
147	dest_addr.sin_family = AF_INET;	×
148
149	inet_pton(AF_INET, _remote_ip.c_str(), &dest_addr.sin_addr.s_addr);	×
150
151	dest_addr.sin_port = htons(_remote_port_number);	×
152
153	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];	×
154	uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);	×
155
156	// TODO: remove this assert again
157	assert(buffer_len <= MAVLINK_MAX_PACKET_LEN);	×
158
159	#if !defined(MSG_NOSIGNAL)
160	auto flags = 0;
161	#else
162	auto flags = MSG_NOSIGNAL;	×
163	#endif
164
165	const auto send_len = sendto(	×
166	_socket_fd.get(),
167	reinterpret_cast<char*>(buffer),
168	buffer_len,
169	flags,
170	reinterpret_cast<const sockaddr*>(&dest_addr),
171	sizeof(dest_addr));
172
173	if (send_len != buffer_len) {	×
174	LogErr() << "sendto failure: " << GET_ERROR(errno);	×
175	_is_ok = false;	×
176	return false;	×
177	}
178	return true;	×
179	}
180
181	void TcpConnection::receive()	×
182	{
183	// Enough for MTU 1500 bytes.
184	char buffer[2048];	×
185
186	while (!_should_exit) {	×
187	if (!_is_ok) {	×
188	LogErr() << "TCP receive error, trying to reconnect...";	×
189	std::this_thread::sleep_for(std::chrono::seconds(1));	×
190	setup_port();	×
191	}
192
193	const auto recv_len = recv(_socket_fd.get(), buffer, sizeof(buffer), 0);	×
194
195	if (recv_len == 0) {	×
196	// This can happen when shutdown is called on the socket,
197	// therefore we check _should_exit again.
198	_is_ok = false;	×
199	continue;	×
200	}
201
202	if (recv_len < 0) {	×
203	// This happens on destruction when close(_socket_fd.get()) is called,
204	// therefore be quiet.
205	// LogErr() << "recvfrom error: " << GET_ERROR(errno);
206	// Something went wrong, we should try to re-connect in next iteration.
207	_is_ok = false;	×
208	continue;	×
209	}
210
211	_mavlink_receiver->set_new_datagram(buffer, static_cast<int>(recv_len));	×
212
213	// Parse all mavlink messages in one data packet. Once exhausted, we'll exit while.
214	while (_mavlink_receiver->parse_message()) {	×
215	receive_message(_mavlink_receiver->get_last_message(), this);	×
216	}
217	}
218	}	×
219
220	} // namespace mavsdk

mavlink / MAVSDK / 11452150858

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous