<commit_before>#include "udp_connection.h"
#include "global_include.h"

#ifndef WINDOWS
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <errno.h>
#else
#include <winsock2.h>
#include <Ws2tcpip.h> // For InetPton
#undef SOCKET_ERROR // conflicts with ConnectionResult::SOCKET_ERROR
#endif

#include <cassert>

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

namespace dronelink {

UdpConnection::UdpConnection(DroneLinkImpl *parent,
                             int local_port_number) :
    Connection(parent),
    _local_port_number(local_port_number),
    _should_exit(false)
{
    if (_local_port_number == 0) {
        _local_port_number = DEFAULT_UDP_LOCAL_PORT;
    }
}

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

bool UdpConnection::is_ok() const
{
    return true;
}

DroneLink::ConnectionResult UdpConnection::start()
{
    if (!start_mavlink_receiver()) {
        return DroneLink::ConnectionResult::CONNECTIONS_EXHAUSTED;
    }

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

    start_recv_thread();

    return DroneLink::ConnectionResult::SUCCESS;
}

DroneLink::ConnectionResult UdpConnection::setup_port()
{

#ifdef WINDOWS
    WSADATA wsa;
    if (WSAStartup(MAKEWORD(2,2), &wsa) != 0)
    {
        Debug() << "Error: Winsock failed, error: %d", WSAGetLastError();
	return DroneLink::ConnectionResult::SOCKET_ERROR;
    }
#endif

    _socket_fd = socket(AF_INET, SOCK_DGRAM, 0);

    if (_socket_fd < 0) {
        Debug() << "socket error" << GET_ERROR(errno);
        return DroneLink::ConnectionResult::SOCKET_ERROR;
    }

    struct sockaddr_in addr;
    memset((char *)&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_port = htons(_local_port_number);
    addr.sin_addr.s_addr = htonl(INADDR_ANY);

    if (bind(_socket_fd, (sockaddr *)&addr, sizeof(addr)) != 0) {
        Debug() << "bind error: " << GET_ERROR(errno);
        return DroneLink::ConnectionResult::BIND_ERROR;
    }

    return DroneLink::ConnectionResult::SUCCESS;
}

void UdpConnection::start_recv_thread()
{
    _recv_thread = new std::thread(receive, this);
}

DroneLink::ConnectionResult UdpConnection::stop()
{
    _should_exit = true;

#ifndef WINDOWS
    // This should interrupt a recv/recvfrom call.
    shutdown(_socket_fd, SHUT_RDWR);

    // But on Mac, closing is also needed to stop blocking recv/recvfrom.
    close(_socket_fd);
#else
    shutdown(_socket_fd, SD_BOTH);

    closesocket(_socket_fd);

    WSACleanup();
#endif

    if (_recv_thread) {
        _recv_thread->join();
        delete _recv_thread;
        _recv_thread = nullptr;
    }

    // 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 DroneLink::ConnectionResult::SUCCESS;
}

bool UdpConnection::send_message(const mavlink_message_t &message)
{
    if (_remote_ip.empty()) {
        Debug() << "Remote IP unknown";
        return false;
    }

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

    struct sockaddr_in dest_addr;
    memset((char *)&dest_addr, 0, sizeof(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);

    int send_len = sendto(_socket_fd, (const char *)buffer, buffer_len, 0,
                          (const sockaddr *)&dest_addr, sizeof(dest_addr));

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

    return true;
}

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

    while (!parent->_should_exit) {

        struct sockaddr_in src_addr = {};
        socklen_t src_addr_len = sizeof(src_addr);
        int recv_len = recvfrom(parent->_socket_fd, buffer, sizeof(buffer), 0,
                                (struct sockaddr *)&src_addr, &src_addr_len);

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

        if (recv_len < 0) {
            // This happens on desctruction when close(_socket_fd) is called,
            // therefore be quiet.
            //Debug() << "recvfrom error: " << GET_ERROR(errno);
            continue;
        }

        int new_remote_port_number = ntohs(src_addr.sin_port);
        std::string new_remote_ip(inet_ntoa(src_addr.sin_addr));

        // TODO make calls to remote threadsafe.

        if (parent->_remote_ip.empty()) {

            if (parent->_remote_port_number == 0 ||
                parent->_remote_port_number == new_remote_port_number) {
                // Set IP if we don't know it yet.
                parent->_remote_ip = new_remote_ip;
                parent->_remote_port_number = new_remote_port_number;

                Debug() << "Partner IP: " << parent->_remote_ip
                        << ":" << parent->_remote_port_number;

            } else {

                Debug() << "Ignoring message from remote port " << new_remote_port_number
                        << " instead of " << parent->_remote_port_number;
                continue;
            }

        } else if (parent->_remote_ip.compare(new_remote_ip) != 0) {
            Debug() << "Ignoring message from IP: " << new_remote_ip;
            continue;

        } else {
            if (parent->_remote_port_number != new_remote_port_number) {
                Debug() << "Ignoring message from remote port " << new_remote_port_number
                        << " instead of " << parent->_remote_port_number;
                continue;
            }
        }

        parent->_mavlink_receiver->set_new_datagram(buffer, recv_len);

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

} // namespace dronelink
<commit_msg>udp_connection.cpp: style fixes<commit_after>#include "udp_connection.h"
#include "global_include.h"

#ifndef WINDOWS
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <errno.h>
#else
#include <winsock2.h>
#include <Ws2tcpip.h> // For InetPton
#undef SOCKET_ERROR // conflicts with ConnectionResult::SOCKET_ERROR
#endif

#include <cassert>

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

namespace dronelink {

UdpConnection::UdpConnection(DroneLinkImpl *parent,
                             int local_port_number) :
    Connection(parent),
    _local_port_number(local_port_number),
    _should_exit(false)
{
    if (_local_port_number == 0) {
        _local_port_number = DEFAULT_UDP_LOCAL_PORT;
    }
}

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

bool UdpConnection::is_ok() const
{
    return true;
}

DroneLink::ConnectionResult UdpConnection::start()
{
    if (!start_mavlink_receiver()) {
        return DroneLink::ConnectionResult::CONNECTIONS_EXHAUSTED;
    }

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

    start_recv_thread();

    return DroneLink::ConnectionResult::SUCCESS;
}

DroneLink::ConnectionResult UdpConnection::setup_port()
{

#ifdef WINDOWS
    WSADATA wsa;
    if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) {
        Debug() << "Error: Winsock failed, error: %d", WSAGetLastError();
        return DroneLink::ConnectionResult::SOCKET_ERROR;
    }
#endif

    _socket_fd = socket(AF_INET, SOCK_DGRAM, 0);

    if (_socket_fd < 0) {
        Debug() << "socket error" << GET_ERROR(errno);
        return DroneLink::ConnectionResult::SOCKET_ERROR;
    }

    struct sockaddr_in addr;
    memset((char *)&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_port = htons(_local_port_number);
    addr.sin_addr.s_addr = htonl(INADDR_ANY);

    if (bind(_socket_fd, (sockaddr *)&addr, sizeof(addr)) != 0) {
        Debug() << "bind error: " << GET_ERROR(errno);
        return DroneLink::ConnectionResult::BIND_ERROR;
    }

    return DroneLink::ConnectionResult::SUCCESS;
}

void UdpConnection::start_recv_thread()
{
    _recv_thread = new std::thread(receive, this);
}

DroneLink::ConnectionResult UdpConnection::stop()
{
    _should_exit = true;

#ifndef WINDOWS
    // This should interrupt a recv/recvfrom call.
    shutdown(_socket_fd, SHUT_RDWR);

    // But on Mac, closing is also needed to stop blocking recv/recvfrom.
    close(_socket_fd);
#else
    shutdown(_socket_fd, SD_BOTH);

    closesocket(_socket_fd);

    WSACleanup();
#endif

    if (_recv_thread) {
        _recv_thread->join();
        delete _recv_thread;
        _recv_thread = nullptr;
    }

    // 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 DroneLink::ConnectionResult::SUCCESS;
}

bool UdpConnection::send_message(const mavlink_message_t &message)
{
    if (_remote_ip.empty()) {
        Debug() << "Remote IP unknown";
        return false;
    }

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

    struct sockaddr_in dest_addr;
    memset((char *)&dest_addr, 0, sizeof(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);

    int send_len = sendto(_socket_fd, (const char *)buffer, buffer_len, 0,
                          (const sockaddr *)&dest_addr, sizeof(dest_addr));

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

    return true;
}

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

    while (!parent->_should_exit) {

        struct sockaddr_in src_addr = {};
        socklen_t src_addr_len = sizeof(src_addr);
        int recv_len = recvfrom(parent->_socket_fd, buffer, sizeof(buffer), 0,
                                (struct sockaddr *)&src_addr, &src_addr_len);

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

        if (recv_len < 0) {
            // This happens on desctruction when close(_socket_fd) is called,
            // therefore be quiet.
            //Debug() << "recvfrom error: " << GET_ERROR(errno);
            continue;
        }

        int new_remote_port_number = ntohs(src_addr.sin_port);
        std::string new_remote_ip(inet_ntoa(src_addr.sin_addr));

        // TODO make calls to remote threadsafe.

        if (parent->_remote_ip.empty()) {

            if (parent->_remote_port_number == 0 ||
                parent->_remote_port_number == new_remote_port_number) {
                // Set IP if we don't know it yet.
                parent->_remote_ip = new_remote_ip;
                parent->_remote_port_number = new_remote_port_number;

                Debug() << "Partner IP: " << parent->_remote_ip
                        << ":" << parent->_remote_port_number;

            } else {

                Debug() << "Ignoring message from remote port " << new_remote_port_number
                        << " instead of " << parent->_remote_port_number;
                continue;
            }

        } else if (parent->_remote_ip.compare(new_remote_ip) != 0) {
            Debug() << "Ignoring message from IP: " << new_remote_ip;
            continue;

        } else {
            if (parent->_remote_port_number != new_remote_port_number) {
                Debug() << "Ignoring message from remote port " << new_remote_port_number
                        << " instead of " << parent->_remote_port_number;
                continue;
            }
        }

        parent->_mavlink_receiver->set_new_datagram(buffer, recv_len);

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

} // namespace dronelink
<|endoftext|>


<commit_before>#pragma once
//=====================================================================//
/*!	@file
	@brief	グラフィックス・カラー定義
    @author 平松邦仁 (hira@rvf-rc45.net)
	@copyright	Copyright (C) 2018, 2019 Kunihito Hiramatsu @n
				Released under the MIT license @n
				https://github.com/hirakuni45/RX/blob/master/LICENSE
*/
//=====================================================================//
#include <cstdint>
#include "common/byte_order.h"

namespace graphics {

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	RGBA カラー型
		@param[in]	T	基本型
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <typename T>
	struct rgba_base_t {

		typedef T value_type;

		/// BGRA for DRW2D d2_color type
#if defined(LITTLE_ENDIAN)
		T		b;
		T		g;
		T		r;
		T		a;
		constexpr rgba_base_t(T rr = 0, T gg = 0, T bb = 0, T aa = 255) :
			b(bb), g(gg), r(rr), a(aa) { }
		constexpr rgba_base_t(const rgba_base_t<T>& t) : b(t.b), g(t.g), r(t.r), a(t.a) { }
#elif defined(BIG_ENDIAN)
		T		a;
		T		r;
		T		g;
		T		b;
		constexpr rgba_base_t(T rr = 0, T gg = 0, T bb = 0, T aa = 255) :
			a(aa), r(rr), g(gg), b(bb) { }
		constexpr rgba_base_t(const rgba_base_t<T>& t) : a(t.a), r(t.r), g(t.g), b(t.b) { }
#endif
	};
	typedef rgba_base_t<uint8_t> rgba8_t;
	typedef rgba_base_t<float  > rgbaf_t;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	カラー型（共有型）
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	union color_t {
		uint32_t	rgba;
		rgba8_t		unit;

		constexpr explicit color_t(uint32_t v = 0xff000000) : rgba(v) { }
		constexpr color_t(const rgba8_t& t) : unit(t) { }
		constexpr color_t(uint8_t r, uint8_t g, uint8_t b, uint8_t a = 255) : unit(r, g, b, a) { }
		color_t(const color_t& t) noexcept : rgba(t.rgba) { }
 		color_t& operator = (const color_t& t) noexcept { rgba = t.rgba; return *this; }
		color_t& operator = (color_t&& t) noexcept { rgba = t.rgba; return *this; }
		color_t& operator *= (uint8_t s) noexcept {
			unit.r = (static_cast<uint16_t>(unit.r) * (static_cast<uint16_t>(s) + 1)) >> 8;
			unit.g = (static_cast<uint16_t>(unit.g) * (static_cast<uint16_t>(s) + 1)) >> 8;
			unit.b = (static_cast<uint16_t>(unit.b) * (static_cast<uint16_t>(s) + 1)) >> 8;
			return *this;
		}
		bool operator == (const color_t& t) const { return rgba == t.rgba; }
		bool operator != (const color_t& t) const { return rgba != t.rgba; }
	};

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	共有カラー・クラス @n
				16 ビットカラー、32 ビットカラー格納クラス
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	struct share_color {

		static constexpr uint16_t to_565(uint8_t r, uint8_t g, uint8_t b)
		{
			return
				(static_cast<uint16_t>(r & 0xf8) << 8)
			  | (static_cast<uint16_t>(g & 0xfc) << 3)
			  | (static_cast<uint16_t>(b) >> 3);
		}

		uint16_t	rgb565;
		color_t		rgba8;

		constexpr share_color(uint8_t r, uint8_t g, uint8_t b) noexcept :
			rgb565(to_565(r, g, b)), rgba8(r, g, b, 255) { }

		share_color(const share_color& t) noexcept :
			rgb565(t.rgb565), rgba8(t.rgba8) { }

		share_color& operator = (const share_color& t) noexcept {
			rgb565 = t.rgb565;
			rgba8 = t.rgba8;
			return *this;
		}

		bool operator == (const share_color& t) const noexcept {
			return rgba8 == t.rgba8;
		}

		bool operator != (const share_color& t) const noexcept {
			return rgba8 != t.rgba8;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief	輝度付きカラー設定
			@param[in]	col	ベースカラー
			@param[in]	y	輝度
		*/
		//-----------------------------------------------------------------//
		void set_color(const color_t& col, uint8_t y) noexcept
		{
			rgba8 = col;
			rgba8 *= y;
			rgb565 = to_565(rgba8.unit.r, rgba8.unit.g, rgba8.unit.b);
		}

		//-----------------------------------------------------------------//
		/*!
			@brief	RGB565 カラーの合成
			@param[in]	c0	カラーＡ
			@param[in]	c1	カラーＢ
			@return 合成されたカラー
		*/
		//-----------------------------------------------------------------//
		static uint16_t color_sum(uint16_t c0, uint16_t c1) noexcept
		{
			uint16_t r = c0 & 0b1111100000000000;
			uint16_t g = c0 & 0b0000011111100000;
			uint16_t b = c0 & 0b0000000000011111;
			r >>= 1;
			r += (c1 & 0b1111100000000000) >> 1;
			r &= 0b1111100000000000;
			g += c1 & 0b0000011111100000;
			g >>= 1;
			g &= 0b0000011111100000;
			b += c1 & 0b0000000000011111;
			b >>= 1;
			b &= 0b0000000000011111;
			return r | g | b;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief	RGB565 カラーから RGBA888 への変換
			@param[in]	src		ソースカラー
			@return 変換されたカラー
		*/
		//-----------------------------------------------------------------//
		static rgba8_t conv_rgba8(uint16_t src) noexcept {
			uint8_t r = (src & 0b1111100000000000) >> 8;
			r |= r >> 5;
			uint8_t g = (src & 0b0000011111100000) >> 3;
			g |= g >> 6;
			uint8_t b = (src & 0b0000000000011111) << 3;
			b |= b >> 5;
			rgba8_t t(r, g, b);
			return t;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief	アルファ・ブレンド
			@param[in]	dst		元カラー
			@param[in]	src		合成カラー
			@return ブレンドされたカラー
		*/
		//-----------------------------------------------------------------//
		static rgba8_t blend(const rgba8_t& dst, const rgba8_t& src) noexcept {
			uint16_t aa = 256 - (src.a + 1);
			uint16_t ba = src.a + 1;
			uint16_t r = (dst.r * aa) + (src.r * ba);
			uint16_t g = (dst.g * aa) + (src.g * ba);
			uint16_t b = (dst.b * aa) + (src.b * ba);
			rgba8_t t(r >> 8, g >> 8, b >> 8);
			return t;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief	アルファ・ブレンド
			@param[in]	fc		正カラー
			@param[in]	alpha	輝度
			@param[in]	bc		裏カラー
			@return 合成後カラー
		*/
		//-----------------------------------------------------------------//
		static rgba8_t blend(const rgba8_t& fc, uint8_t alpha, const rgba8_t& bc) noexcept
		{
			uint16_t fi = alpha + 1;
			uint16_t bi = 256 - (alpha + 1);
			uint8_t r = (static_cast<uint16_t>(fc.r) * fi + static_cast<uint16_t>(bc.r) * bi) >> 8;
			uint8_t g = (static_cast<uint16_t>(fc.g) * fi + static_cast<uint16_t>(bc.g) * bi) >> 8;
			uint8_t b = (static_cast<uint16_t>(fc.b) * fi + static_cast<uint16_t>(bc.b) * bi) >> 8;
			return rgba8_t(r, g, b);
		}
	};

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	カラー定義
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	struct def_color {
		static const share_color Black;
		static const share_color Maroon;
		static const share_color Green;
		static const share_color Olive;
		static const share_color Navy;
		static const share_color Purple;
		static const share_color Teal;
		static const share_color Silver;
		static const share_color Gray;
		static const share_color Grey;
		static const share_color Midgray;
		static const share_color Darkgray;
		static const share_color Red;
		static const share_color Lime;
		static const share_color Yellow;
		static const share_color Blue;
		static const share_color Fuchsi;
		static const share_color Aqua;
		static const share_color White;
		static const share_color Orange;
		static const share_color SafeColor;
		static const share_color EmeraldGreen;
		static const share_color LightPink;
		static const share_color DarkSafeColor;
		static const share_color LightSafeColor;
	};
}
<commit_msg>Update: cleanup<commit_after>#pragma once
//=====================================================================//
/*!	@file
	@brief	グラフィックス・カラー定義
    @author 平松邦仁 (hira@rvf-rc45.net)
	@copyright	Copyright (C) 2018, 2019 Kunihito Hiramatsu @n
				Released under the MIT license @n
				https://github.com/hirakuni45/RX/blob/master/LICENSE
*/
//=====================================================================//
#include <cstdint>
#include "common/byte_order.h"

namespace graphics {

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	RGBA カラー型
		@param[in]	T	基本型
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	template <typename T>
	struct rgba_base_t {

		typedef T value_type;

		/// BGRA for DRW2D d2_color type
#if defined(LITTLE_ENDIAN)
		T		b;
		T		g;
		T		r;
		T		a;
		constexpr rgba_base_t(T rr = 0, T gg = 0, T bb = 0, T aa = 255) :
			b(bb), g(gg), r(rr), a(aa) { }
		constexpr rgba_base_t(const rgba_base_t<T>& t) : b(t.b), g(t.g), r(t.r), a(t.a) { }
#elif defined(BIG_ENDIAN)
		T		a;
		T		r;
		T		g;
		T		b;
		constexpr rgba_base_t(T rr = 0, T gg = 0, T bb = 0, T aa = 255) :
			a(aa), r(rr), g(gg), b(bb) { }
		constexpr rgba_base_t(const rgba_base_t<T>& t) : a(t.a), r(t.r), g(t.g), b(t.b) { }
#endif
	};
	typedef rgba_base_t<uint8_t> rgba8_t;
	typedef rgba_base_t<float  > rgbaf_t;

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	カラー型（共有型）
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	union color_t {
		uint32_t	rgba;
		rgba8_t		unit;

		constexpr explicit color_t(uint32_t v = 0xff000000) : rgba(v) { }
		constexpr color_t(const rgba8_t& t) : unit(t) { }
		constexpr color_t(uint8_t r, uint8_t g, uint8_t b, uint8_t a = 255) : unit(r, g, b, a) { }
		constexpr color_t(const color_t& t) noexcept : rgba(t.rgba) { }
 		color_t& operator = (const color_t& t) noexcept { rgba = t.rgba; return *this; }
		color_t& operator = (color_t&& t) noexcept { rgba = t.rgba; return *this; }
		color_t& operator *= (uint8_t s) noexcept {
			unit.r = (static_cast<uint16_t>(unit.r) * (static_cast<uint16_t>(s) + 1)) >> 8;
			unit.g = (static_cast<uint16_t>(unit.g) * (static_cast<uint16_t>(s) + 1)) >> 8;
			unit.b = (static_cast<uint16_t>(unit.b) * (static_cast<uint16_t>(s) + 1)) >> 8;
			return *this;
		}
		bool operator == (const color_t& t) const { return rgba == t.rgba; }
		bool operator != (const color_t& t) const { return rgba != t.rgba; }
	};

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	共有カラー・クラス @n
				16 ビットカラー、32 ビットカラー格納クラス
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	struct share_color {

		static constexpr uint16_t to_565(uint8_t r, uint8_t g, uint8_t b)
		{
			return
				(static_cast<uint16_t>(r & 0xf8) << 8)
			  | (static_cast<uint16_t>(g & 0xfc) << 3)
			  | (static_cast<uint16_t>(b) >> 3);
		}

		uint16_t	rgb565;
		color_t		rgba8;

		constexpr share_color(uint8_t r, uint8_t g, uint8_t b) noexcept :
			rgb565(to_565(r, g, b)), rgba8(r, g, b, 255) { }

		constexpr share_color(const share_color& t) noexcept :
			rgb565(t.rgb565), rgba8(t.rgba8) { }

		share_color& operator = (const share_color& t) noexcept {
			rgb565 = t.rgb565;
			rgba8 = t.rgba8;
			return *this;
		}

		bool operator == (const share_color& t) const noexcept {
			return rgba8 == t.rgba8;
		}

		bool operator != (const share_color& t) const noexcept {
			return rgba8 != t.rgba8;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief	輝度付きカラー設定
			@param[in]	col	ベースカラー
			@param[in]	y	輝度
		*/
		//-----------------------------------------------------------------//
		void set_color(const color_t& col, uint8_t y) noexcept
		{
			rgba8 = col;
			rgba8 *= y;
			rgb565 = to_565(rgba8.unit.r, rgba8.unit.g, rgba8.unit.b);
		}

		//-----------------------------------------------------------------//
		/*!
			@brief	RGB565 カラーの合成
			@param[in]	c0	カラーＡ
			@param[in]	c1	カラーＢ
			@return 合成されたカラー
		*/
		//-----------------------------------------------------------------//
		static uint16_t color_sum(uint16_t c0, uint16_t c1) noexcept
		{
			uint16_t r = c0 & 0b1111100000000000;
			uint16_t g = c0 & 0b0000011111100000;
			uint16_t b = c0 & 0b0000000000011111;
			r >>= 1;
			r += (c1 & 0b1111100000000000) >> 1;
			r &= 0b1111100000000000;
			g += c1 & 0b0000011111100000;
			g >>= 1;
			g &= 0b0000011111100000;
			b += c1 & 0b0000000000011111;
			b >>= 1;
			b &= 0b0000000000011111;
			return r | g | b;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief	RGB565 カラーから RGBA888 への変換
			@param[in]	src		ソースカラー
			@return 変換されたカラー
		*/
		//-----------------------------------------------------------------//
		static rgba8_t conv_rgba8(uint16_t src) noexcept {
			uint8_t r = (src & 0b1111100000000000) >> 8;
			r |= r >> 5;
			uint8_t g = (src & 0b0000011111100000) >> 3;
			g |= g >> 6;
			uint8_t b = (src & 0b0000000000011111) << 3;
			b |= b >> 5;
			rgba8_t t(r, g, b);
			return t;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief	アルファ・ブレンド
			@param[in]	dst		元カラー
			@param[in]	src		合成カラー
			@return ブレンドされたカラー
		*/
		//-----------------------------------------------------------------//
		static rgba8_t blend(const rgba8_t& dst, const rgba8_t& src) noexcept {
			uint16_t aa = 256 - (src.a + 1);
			uint16_t ba = src.a + 1;
			uint16_t r = (dst.r * aa) + (src.r * ba);
			uint16_t g = (dst.g * aa) + (src.g * ba);
			uint16_t b = (dst.b * aa) + (src.b * ba);
			rgba8_t t(r >> 8, g >> 8, b >> 8);
			return t;
		}

		//-----------------------------------------------------------------//
		/*!
			@brief	アルファ・ブレンド
			@param[in]	fc		正カラー
			@param[in]	alpha	輝度
			@param[in]	bc		裏カラー
			@return 合成後カラー
		*/
		//-----------------------------------------------------------------//
		static rgba8_t blend(const rgba8_t& fc, uint8_t alpha, const rgba8_t& bc) noexcept
		{
			uint16_t fi = alpha + 1;
			uint16_t bi = 256 - (alpha + 1);
			uint8_t r = (static_cast<uint16_t>(fc.r) * fi + static_cast<uint16_t>(bc.r) * bi) >> 8;
			uint8_t g = (static_cast<uint16_t>(fc.g) * fi + static_cast<uint16_t>(bc.g) * bi) >> 8;
			uint8_t b = (static_cast<uint16_t>(fc.b) * fi + static_cast<uint16_t>(bc.b) * bi) >> 8;
			return rgba8_t(r, g, b);
		}
	};

	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	/*!
		@brief	カラー定義
	*/
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++//
	struct def_color {
		static const share_color Black;
		static const share_color Maroon;
		static const share_color Green;
		static const share_color Olive;
		static const share_color Navy;
		static const share_color Purple;
		static const share_color Teal;
		static const share_color Silver;
		static const share_color Gray;
		static const share_color Grey;
		static const share_color Midgray;
		static const share_color Darkgray;
		static const share_color Red;
		static const share_color Lime;
		static const share_color Yellow;
		static const share_color Blue;
		static const share_color Fuchsi;
		static const share_color Aqua;
		static const share_color White;
		static const share_color Orange;
		static const share_color SafeColor;
		static const share_color EmeraldGreen;
		static const share_color LightPink;
		static const share_color DarkSafeColor;
		static const share_color LightSafeColor;
	};
}
<|endoftext|>


<commit_before>#include "gridmap.hpp"
#include "../utils/utils.hpp"
#include "../utils/safeops.hpp"
#include <cstdio>
#include <cstddef>
#include <cerrno>
#include <cstring>
#include <cstdarg>
#include <cmath>
#include <string>

GridMap::GridMap(unsigned int width, unsigned int height) :
	map(NULL), file(""), lifecost(false), nmvs(0), flags(NULL) {
	try {
		setsize(width, height);
	} catch (const safe::BadFlow<unsigned int>&) {
		fatal("Grid is too big");
	}
	setfourway();
}

GridMap::GridMap(std::string &fname) :
	map(NULL),
	file(fname),
	lifecost(false),
	nmvs(0),
	flags(NULL) {

	FILE *f = fopen(file.c_str(), "r");
	if (!f)
		fatalx(errno, "Unable to open %s for reading\n", file.c_str());
	load(f);
	fclose(f);
}

GridMap GridMap::uniform(unsigned int w, unsigned int h,
		double prob, uint64_t seed) {
	GridMap m(w, h);

	Rand rng(seed);
	for (unsigned int x = 1; x < w+1; x++) {
	for (unsigned int y = 1; y < h+1; y++) {
		int i = m.index(x, y);
		if (rng.real() < prob) {
			m.map[i] = '#';
			m.flags[i] = OutOfBounds;
		} else {
 			m.map[i] = ' ';
			m.flags[i] = Passable;
		}
	}
	}

	return m;
}

GridMap::~GridMap() {
	if (map)
		delete[] map;
	if (flags)
		delete[] flags;
}

void GridMap::readfail(const char *fmt, ...) {
	va_list ap;

	va_start(ap, fmt);
	unsigned int sz = vsnprintf(NULL, 0, fmt, ap);
	va_end(ap);

	char buf[sz+1];
	va_start(ap, fmt);
	vsnprintf(buf, sz+1, fmt, ap);
	va_end(ap);

 	if (file.size() > 0)
		fatal("%s: %s", file.c_str(), buf);
	fatal("%s", buf);
}

void GridMap::load(FILE *in) {
	int c = fgetc(in);
	ungetc(c, in);

	if (c == 't')
		load_sturtevant(in);
	else if (c == 's')
		load_seedinst(in);
	else if (std::isdigit(c))
		load_ruml(in);
	else
		readfail("Failed to read the map header, expected 't' or a digit");
}

void GridMap::load_seedinst(FILE *in) {
	unsigned long seed;
	if (sizeof(seed) < sizeof(uint64_t))
		fatal("unsigned long is too small to read 64-bit seeds");

	double prob;
	if (fscanf(in, " seed %lu %u %u %lg\n", &seed, &w, &h, &prob) != 4)
		readfail("Failed to read map header");

	int c = fgetc(in);
	ungetc(c, in);
	if (c == 'E' && fscanf(in, "Eightway\n") == 0)
		seteightway();
	else if (c == 'F' && fscanf(in, "Fourway\n") == 0)
		setfourway();
	else
		readfail("Invalid movement type");

	c = fgetc(in);
	ungetc(c, in);
	if (c == 'U' && fscanf(in, "Unit\n") == 0)
		lifecost = false;
	else if (c == 'L' && fscanf(in, "Life\n") == 0)
		lifecost = true;
	else
		readfail("Invalid cost type");

	GridMap r = uniform(w, h, prob, seed);
	w = r.w;
	h = r.h;
	sz = r.sz;
	map = r.map;
	flags = r.flags;
	r.map = NULL;
	r.flags = NULL;
}

// Leaves the start/end locations in the FILE.
void GridMap::load_ruml(FILE *in) {
	if (fscanf(in, " %u %u\nBoard:", &h, &w) != 2)
		readfail("Failed to read map size");

	setsize(w, h);

	if (fgetc(in) != '\n')
		fatal("Expected a new line after 'Board'");

	for (unsigned int y = 1; y < h-1; y++) {
		for (unsigned int x = 1; x < w-1; x++) {
	 		unsigned int i = (h - y - 1) * w + x;
			int c = fgetc(in);
			if (c == EOF && ferror(in))
				readfail("Failed to read the board");
			if (c == EOF)
				readfail("Failed to read the board: unexpected end of file");
			map[i] = c;
			if (c == ' ')
				flags[i] = Passable;
			else if (c == '#')
				flags[i] = OutOfBounds;
			else
				readfail("x=%u, y=%u: Unexpected character [%c] in board description",
					x, y, c);
		}
		if (fgetc(in) != '\n')
			readfail("Expected a new line");
	}

	int c = fgetc(in);
	ungetc(c, in);
	if (c == 'U' && fscanf(in, " Unit\n") == 0)
		lifecost = false;
	else if (c == 'L' && fscanf(in, " Life\n") == 0)
		lifecost = true;
	else
		readfail("Invalid cost type");

	c = fgetc(in);
	ungetc(c, in);
	if (c == 'E' && fscanf(in, "Eight-way") == 0)
		seteightway();
	else if (c == 'F' && fscanf(in, "Four-way") == 0)
		setfourway();
	else
		readfail("Invalid movement type");
}

void GridMap::load_sturtevant(FILE *in) {
	if (fscanf(in, "type ") != 0)
		readfail("Failed to read the map type");

	char typ[128];
	if (!fgets(typ, sizeof(typ), in))
		readfail("Failed to read the map type");
	if (typ[strlen(typ)-1] == '\n')
		typ[strlen(typ)-1] = '\0';
	if (strcmp(typ, "octile") != 0)
		readfail("Unsupported map type [%s]", typ);

	if (fscanf(in, "height %u\nwidth %u\nmap\n", &h, &w) != 2)
		readfail("Failed to read the map header");

	setsize(w, h);

	for (unsigned int y = 1; y < h-1; y++) {
		if (fread((void*) (map + y * w + 1), sizeof(*map), w-2, in) != w-2)
			readfail("Failed to read map line %u", y);
		int c = fgetc(in);
		if (c != '\n' && y < h - 1)
			readfail("Expected newline, got [%c]",c);
	}

	const struct Terrain{
		char flags[CHAR_MAX];
		Terrain() {
			flags[(int) '.'] = Passable;
			flags[(int) 'G'] = Passable;
			flags[(int) '@'] = OutOfBounds;
			flags[(int) 'O'] = OutOfBounds;
	 		flags[(int) 'T'] = Tree;
			flags[(int) 'S'] = Swamp;
			flags[(int) 'W'] = Water;
		}
	} terrain;

	for (unsigned int y = 1; y < h-1; y++) {
		for (unsigned int x = 1; x < w-1; x++)
			flags[y*w + x] = terrain.flags[(int) map[y*w + x]];
	}

	setoctile();
}

void GridMap::setsize(unsigned int width, unsigned int height) {
	w = safe::add(width, (unsigned int) 2);
	h = safe::add(height, (unsigned int) 2);
	sz = safe::mul(w, h);
	map = new unsigned char[sz];
	flags = new unsigned char[sz];

	for (unsigned int i = 0; i < sz; i++) {
		map[i] = ' ';
		flags[i] = Passable;
	}

	for (unsigned int i = 0; i < w; i++) {
		map[index(i, 0)] = '\0';
		map[index(i, h-1)] = '\0';
		flags[index(i, 0)] = OutOfBounds;
		flags[index(i, h-1)] = OutOfBounds;
	}
	for (unsigned int i = 0; i < h; i++) {
		map[index(0, i)] = '\0';
		map[index(w-1, i)] = '\0';
		flags[index(0, i)] = OutOfBounds;
		flags[index(w-1, i)] = OutOfBounds;
	}
}

GridMap::Move::Move(const GridMap &m, int deltax, int deltay, unsigned int num, ...) :
			dx(deltax), dy(deltay), delta(dx + m.w * dy), cost(1.0), n(num + 1) {
	if (n > sizeof(chk) / sizeof(chk[0]))
		fatal("Cannot create a move with %d checks\n", n);

	if (dx != 0 && dy != 0)
		cost = sqrt(2.0);

	va_list ap;
	va_start(ap, num);
	chk[0].dx = dx;
	chk[0].dy = dy;
	chk[0].delta = delta;
	for (unsigned int i = 1; i < n; i++) {
		chk[i].dx = va_arg(ap, int);
		chk[i].dy = va_arg(ap, int);
		chk[i].delta = chk[i].dx + m.w * chk[i].dy;
	}
	va_end(ap);
}

void GridMap::setoctile() {
	// This operator ordering seems to give more accurate
	// path-costs (compared to Nathan's scenario costs)
	// when simply using doubles as the cost type.
	nmvs = 0;
	mvs[nmvs++] = Move(*this, -1,0, 0);
	mvs[nmvs++] = Move(*this, 1,0, 0);
	mvs[nmvs++] = Move(*this, 0,-1, 0);
	mvs[nmvs++] = Move(*this, -1,-1, 2, 0,-1, -1,0);
	mvs[nmvs++] = Move(*this, 1,-1, 2, 0,-1, 1,0);
	mvs[nmvs++] = Move(*this, 0,1, 0);
	mvs[nmvs++] = Move(*this, -1,1, 2, -1,0, 0,1);
	mvs[nmvs++] = Move(*this, 1,1, 2, 1,0, 0,1);
}

void GridMap::seteightway() {
	nmvs = 0;
	mvs[nmvs++] = Move(*this, 1,1, 0);
	mvs[nmvs++] = Move(*this, 1,-1, 0);
	mvs[nmvs++] = Move(*this, -1,1, 0);
	mvs[nmvs++] = Move(*this, -1,-1, 0);
	mvs[nmvs++] = Move(*this, 0,1, 0);
	mvs[nmvs++] = Move(*this, 0,-1, 0);
	mvs[nmvs++] = Move(*this, -1,0, 0);
	mvs[nmvs++] = Move(*this, 1,0, 0);
}

void GridMap::setfourway() {
	nmvs = 0;
	mvs[nmvs++] = Move(*this, 0,1, 0);
	mvs[nmvs++] = Move(*this, 0,-1, 0);
	mvs[nmvs++] = Move(*this, -1,0, 0);
	mvs[nmvs++] = Move(*this, 1,0, 0);
}
<commit_msg>gridnav: set the 'way' after setting the correct width and height.<commit_after>#include "gridmap.hpp"
#include "../utils/utils.hpp"
#include "../utils/safeops.hpp"
#include <cstdio>
#include <cstddef>
#include <cerrno>
#include <cstring>
#include <cstdarg>
#include <cmath>
#include <string>

GridMap::GridMap(unsigned int width, unsigned int height) :
	map(NULL), file(""), lifecost(false), nmvs(0), flags(NULL) {
	try {
		setsize(width, height);
	} catch (const safe::BadFlow<unsigned int>&) {
		fatal("Grid is too big");
	}
	setfourway();
}

GridMap::GridMap(std::string &fname) :
	map(NULL),
	file(fname),
	lifecost(false),
	nmvs(0),
	flags(NULL) {

	FILE *f = fopen(file.c_str(), "r");
	if (!f)
		fatalx(errno, "Unable to open %s for reading\n", file.c_str());
	load(f);
	fclose(f);
}

GridMap GridMap::uniform(unsigned int w, unsigned int h,
		double prob, uint64_t seed) {
	GridMap m(w, h);

	Rand rng(seed);
	for (unsigned int x = 1; x < w+1; x++) {
	for (unsigned int y = 1; y < h+1; y++) {
		int i = m.index(x, y);
		if (rng.real() < prob) {
			m.map[i] = '#';
			m.flags[i] = OutOfBounds;
		} else {
 			m.map[i] = ' ';
			m.flags[i] = Passable;
		}
	}
	}

	return m;
}

GridMap::~GridMap() {
	if (map)
		delete[] map;
	if (flags)
		delete[] flags;
}

void GridMap::readfail(const char *fmt, ...) {
	va_list ap;

	va_start(ap, fmt);
	unsigned int sz = vsnprintf(NULL, 0, fmt, ap);
	va_end(ap);

	char buf[sz+1];
	va_start(ap, fmt);
	vsnprintf(buf, sz+1, fmt, ap);
	va_end(ap);

 	if (file.size() > 0)
		fatal("%s: %s", file.c_str(), buf);
	fatal("%s", buf);
}

void GridMap::load(FILE *in) {
	int c = fgetc(in);
	ungetc(c, in);

	if (c == 't')
		load_sturtevant(in);
	else if (c == 's')
		load_seedinst(in);
	else if (std::isdigit(c))
		load_ruml(in);
	else
		readfail("Failed to read the map header, expected 't' or a digit");
}

void GridMap::load_seedinst(FILE *in) {
	unsigned long seed;
	if (sizeof(seed) < sizeof(uint64_t))
		fatal("unsigned long is too small to read 64-bit seeds");

	double prob;
	if (fscanf(in, " seed %lu %u %u %lg\n", &seed, &w, &h, &prob) != 4)
		readfail("Failed to read map header");

	int c = fgetc(in);
	ungetc(c, in);
	bool eightway = false;
	if (c == 'E' && fscanf(in, "Eightway\n") == 0)
		eightway = true;
	else if (c == 'F' && fscanf(in, "Fourway\n") == 0)
		eightway = false;
	else
		readfail("Invalid movement type");

	c = fgetc(in);
	ungetc(c, in);
	if (c == 'U' && fscanf(in, "Unit\n") == 0)
		lifecost = false;
	else if (c == 'L' && fscanf(in, "Life\n") == 0)
		lifecost = true;
	else
		readfail("Invalid cost type");

	GridMap r = uniform(w, h, prob, seed);
	w = r.w;
	h = r.h;
	sz = r.sz;
	map = r.map;
	flags = r.flags;
	r.map = NULL;
	r.flags = NULL;

	if (eightway)
		seteightway();
	else
		setfourway();
}

// Leaves the start/end locations in the FILE.
void GridMap::load_ruml(FILE *in) {
	if (fscanf(in, " %u %u\nBoard:", &h, &w) != 2)
		readfail("Failed to read map size");

	setsize(w, h);

	if (fgetc(in) != '\n')
		fatal("Expected a new line after 'Board'");

	for (unsigned int y = 1; y < h-1; y++) {
		for (unsigned int x = 1; x < w-1; x++) {
	 		unsigned int i = (h - y - 1) * w + x;
			int c = fgetc(in);
			if (c == EOF && ferror(in))
				readfail("Failed to read the board");
			if (c == EOF)
				readfail("Failed to read the board: unexpected end of file");
			map[i] = c;
			if (c == ' ')
				flags[i] = Passable;
			else if (c == '#')
				flags[i] = OutOfBounds;
			else
				readfail("x=%u, y=%u: Unexpected character [%c] in board description",
					x, y, c);
		}
		if (fgetc(in) != '\n')
			readfail("Expected a new line");
	}

	int c = fgetc(in);
	ungetc(c, in);
	if (c == 'U' && fscanf(in, " Unit\n") == 0)
		lifecost = false;
	else if (c == 'L' && fscanf(in, " Life\n") == 0)
		lifecost = true;
	else
		readfail("Invalid cost type");

	c = fgetc(in);
	ungetc(c, in);
	if (c == 'E' && fscanf(in, "Eight-way") == 0)
		seteightway();
	else if (c == 'F' && fscanf(in, "Four-way") == 0)
		setfourway();
	else
		readfail("Invalid movement type");
}

void GridMap::load_sturtevant(FILE *in) {
	if (fscanf(in, "type ") != 0)
		readfail("Failed to read the map type");

	char typ[128];
	if (!fgets(typ, sizeof(typ), in))
		readfail("Failed to read the map type");
	if (typ[strlen(typ)-1] == '\n')
		typ[strlen(typ)-1] = '\0';
	if (strcmp(typ, "octile") != 0)
		readfail("Unsupported map type [%s]", typ);

	if (fscanf(in, "height %u\nwidth %u\nmap\n", &h, &w) != 2)
		readfail("Failed to read the map header");

	setsize(w, h);

	for (unsigned int y = 1; y < h-1; y++) {
		if (fread((void*) (map + y * w + 1), sizeof(*map), w-2, in) != w-2)
			readfail("Failed to read map line %u", y);
		int c = fgetc(in);
		if (c != '\n' && y < h - 1)
			readfail("Expected newline, got [%c]",c);
	}

	const struct Terrain{
		char flags[CHAR_MAX];
		Terrain() {
			flags[(int) '.'] = Passable;
			flags[(int) 'G'] = Passable;
			flags[(int) '@'] = OutOfBounds;
			flags[(int) 'O'] = OutOfBounds;
	 		flags[(int) 'T'] = Tree;
			flags[(int) 'S'] = Swamp;
			flags[(int) 'W'] = Water;
		}
	} terrain;

	for (unsigned int y = 1; y < h-1; y++) {
		for (unsigned int x = 1; x < w-1; x++)
			flags[y*w + x] = terrain.flags[(int) map[y*w + x]];
	}

	setoctile();
}

void GridMap::setsize(unsigned int width, unsigned int height) {
	w = safe::add(width, (unsigned int) 2);
	h = safe::add(height, (unsigned int) 2);
	sz = safe::mul(w, h);
	map = new unsigned char[sz];
	flags = new unsigned char[sz];

	for (unsigned int i = 0; i < sz; i++) {
		map[i] = ' ';
		flags[i] = Passable;
	}

	for (unsigned int i = 0; i < w; i++) {
		map[index(i, 0)] = '\0';
		map[index(i, h-1)] = '\0';
		flags[index(i, 0)] = OutOfBounds;
		flags[index(i, h-1)] = OutOfBounds;
	}
	for (unsigned int i = 0; i < h; i++) {
		map[index(0, i)] = '\0';
		map[index(w-1, i)] = '\0';
		flags[index(0, i)] = OutOfBounds;
		flags[index(w-1, i)] = OutOfBounds;
	}
}

GridMap::Move::Move(const GridMap &m, int deltax, int deltay, unsigned int num, ...) :
			dx(deltax), dy(deltay), delta(dx + m.w * dy), cost(1.0), n(num + 1) {
	if (n > sizeof(chk) / sizeof(chk[0]))
		fatal("Cannot create a move with %d checks\n", n);

	if (dx != 0 && dy != 0)
		cost = sqrt(2.0);

	va_list ap;
	va_start(ap, num);
	chk[0].dx = dx;
	chk[0].dy = dy;
	chk[0].delta = delta;
	for (unsigned int i = 1; i < n; i++) {
		chk[i].dx = va_arg(ap, int);
		chk[i].dy = va_arg(ap, int);
		chk[i].delta = chk[i].dx + m.w * chk[i].dy;
	}
	va_end(ap);
}

void GridMap::setoctile() {
	// This operator ordering seems to give more accurate
	// path-costs (compared to Nathan's scenario costs)
	// when simply using doubles as the cost type.
	nmvs = 0;
	mvs[nmvs++] = Move(*this, -1,0, 0);
	mvs[nmvs++] = Move(*this, 1,0, 0);
	mvs[nmvs++] = Move(*this, 0,-1, 0);
	mvs[nmvs++] = Move(*this, -1,-1, 2, 0,-1, -1,0);
	mvs[nmvs++] = Move(*this, 1,-1, 2, 0,-1, 1,0);
	mvs[nmvs++] = Move(*this, 0,1, 0);
	mvs[nmvs++] = Move(*this, -1,1, 2, -1,0, 0,1);
	mvs[nmvs++] = Move(*this, 1,1, 2, 1,0, 0,1);
}

void GridMap::seteightway() {
	nmvs = 0;
	mvs[nmvs++] = Move(*this, 1,1, 0);
	mvs[nmvs++] = Move(*this, 1,-1, 0);
	mvs[nmvs++] = Move(*this, -1,1, 0);
	mvs[nmvs++] = Move(*this, -1,-1, 0);
	mvs[nmvs++] = Move(*this, 0,1, 0);
	mvs[nmvs++] = Move(*this, 0,-1, 0);
	mvs[nmvs++] = Move(*this, -1,0, 0);
	mvs[nmvs++] = Move(*this, 1,0, 0);
}

void GridMap::setfourway() {
	nmvs = 0;
	mvs[nmvs++] = Move(*this, 0,1, 0);
	mvs[nmvs++] = Move(*this, 0,-1, 0);
	mvs[nmvs++] = Move(*this, -1,0, 0);
	mvs[nmvs++] = Move(*this, 1,0, 0);
}
<|endoftext|>


<commit_before>#include <gtest/gtest.h>
#include <ros/ros.h>
#include <boost/scoped_ptr.hpp>
#include <roscpp_message_reflection/message_description_provider.h>

using namespace roscpp_message_reflection;

#define EXPECT_VALUE_FIELD(expected_name, expected_type, field)	\
  EXPECT_EQ(expected_name, field.name());			\
  EXPECT_EQ(expected_type, field.full_type());			\
  EXPECT_EQ(expected_type, field.value_type());			\
  EXPECT_EQ(FieldDescription::Value, field.type())

#define EXPECT_VARIABLE_LENGTH_ARRAY_FIELD(expected_name, expected_value_type, field) \
  EXPECT_EQ(expected_name, field.name());				\
  {									\
    std::stringstream expected_full_type_ss;				\
    expected_full_type_ss << expected_value_type;			\
    expected_full_type_ss << "[]";					\
    EXPECT_EQ(expected_full_type_ss.str(), field.full_type());		\
  }									\
  EXPECT_EQ(expected_value_type, field.value_type());			\
  EXPECT_EQ(FieldDescription::VariableLengthArray, field.type());	\

#define EXPECT_FIXED_LENGTH_ARRAY_FIELD(expected_name, expected_value_type, expected_length, field) \
  EXPECT_EQ(expected_name, field.name());				\
  {									\
    std::stringstream expected_full_type_ss;				\
    expected_full_type_ss << expected_value_type;			\
    expected_full_type_ss << "[" << expected_length << "]";		\
    EXPECT_EQ(expected_full_type_ss.str(), field.full_type());		\
  }									\
  EXPECT_EQ(expected_value_type, field.value_type());			\
  EXPECT_EQ(FieldDescription::FixedLengthArray, field.type());		\
  EXPECT_EQ(expected_length, field.array_length())

TEST(MessageDescription, construct_value_field)
{
  FieldDescription field = FieldDescription::CreateFromFullType("some_name", "AType");
  EXPECT_VALUE_FIELD("some_name", "AType", field);
}

TEST(MessageDescription, construct_variable_length_array_field)
{
  FieldDescription field = FieldDescription::CreateFromFullType("some_name", "AType[]");
  EXPECT_VARIABLE_LENGTH_ARRAY_FIELD("some_name", "AType", field);
}

TEST(MessageDescription, construct_fixed_length_array_field)
{
  FieldDescription field = FieldDescription::CreateFromFullType("some_name", "AType[10]");
  EXPECT_FIXED_LENGTH_ARRAY_FIELD("some_name", "AType", 10, field);
}

TEST(MessageDescription, construct_simple_message)
{
  std::vector<FieldDescription> fields;
  fields.push_back(FieldDescription::CreateFromFullType("a", "int32"));
  fields.push_back(FieldDescription::CreateFromFullType("b", "uint16"));
  fields.push_back(FieldDescription::CreateFromFullType("d", "string"));
  MessageDescription message("a_package/AMessage", "992ce8a1687cec8c8bd883ec73ca1234", "int32 a\nuint16 b\nstring d\n\n", fields, std::map<std::string, MessageDescription::Ptr>());
  EXPECT_EQ("a_package/AMessage", message.name);
  EXPECT_EQ("992ce8a1687cec8c8bd883ec73ca1234", message.md5sum);
  EXPECT_EQ("int32 a\nuint16 b\nstring d\n\n", message.full_text);
  ASSERT_EQ(3, message.fields.size());
  EXPECT_VALUE_FIELD("a", "int32", message.fields[0]);
  EXPECT_VALUE_FIELD("b", "uint16", message.fields[1]);
  EXPECT_VALUE_FIELD("d", "string", message.fields[2]);
}

class MessageDescriptionProviderTest : public ::testing::Test {
protected:
  virtual void SetUp() {
    provider.reset(MessageDescriptionProvider::Create(nh));
  }
  ros::NodeHandle nh;
  boost::scoped_ptr<MessageDescriptionProvider> provider;
};

TEST_F(MessageDescriptionProviderTest, get_std_msgs_String_description)
{
  MessageDescription::Ptr description = provider->getDescription("std_msgs/String");
  ASSERT_TRUE(description);
  EXPECT_EQ("std_msgs/String", description->name);
  EXPECT_EQ("992ce8a1687cec8c8bd883ec73ca41d1", description->md5sum);
  EXPECT_EQ("string data\n\n", description->full_text);

  ASSERT_EQ(1, description->fields.size());
  EXPECT_VALUE_FIELD("data", "string", description->fields[0]);
}

TEST_F(MessageDescriptionProviderTest, get_geometry_msgs_Vector3_description)
{
  MessageDescription::Ptr description = provider->getDescription("geometry_msgs/Vector3");
  ASSERT_TRUE(description);
  EXPECT_EQ("geometry_msgs/Vector3", description->name);
  EXPECT_EQ("4a842b65f413084dc2b10fb484ea7f17", description->md5sum);
  EXPECT_EQ("# This represents a vector in free space. \n\nfloat64 x\nfloat64 y\nfloat64 z\n", description->full_text);

  ASSERT_EQ(3, description->fields.size());
  EXPECT_VALUE_FIELD("x", "float64", description->fields[0]);
  EXPECT_VALUE_FIELD("y", "float64", description->fields[1]);
  EXPECT_VALUE_FIELD("z", "float64", description->fields[2]);
}

// Tests expansion of type without package and fixed length array
TEST_F(MessageDescriptionProviderTest, get_geometry_msgs_AccelWithCovariance_description)
{
  MessageDescription::Ptr description = provider->getDescription("geometry_msgs/AccelWithCovariance");
  ASSERT_TRUE(description);
  EXPECT_EQ("geometry_msgs/AccelWithCovariance", description->name);
  EXPECT_EQ("ad5a718d699c6be72a02b8d6a139f334", description->md5sum);
  EXPECT_EQ("# This expresses acceleration in free space with uncertainty.\n\nAccel accel\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Accel\n# This expresses acceleration in free space broken into its linear and angular parts.\nVector3  linear\nVector3  angular\n\n================================================================================\nMSG: geometry_msgs/Vector3\n# This represents a vector in free space. \n\nfloat64 x\nfloat64 y\nfloat64 z\n", description->full_text);

  ASSERT_EQ(2, description->fields.size());
  EXPECT_VALUE_FIELD("accel", "geometry_msgs/Accel", description->fields[0]);
  EXPECT_FIXED_LENGTH_ARRAY_FIELD("covariance", "float64", 36, description->fields[1]);
}

// Tests expansion of type without package recursivly
TEST_F(MessageDescriptionProviderTest, get_geometry_msgs_AccelWithCovarianceStamped_description)
{
  MessageDescription::Ptr description = provider->getDescription("geometry_msgs/AccelWithCovarianceStamped");
  ASSERT_TRUE(description);
  EXPECT_EQ("geometry_msgs/AccelWithCovarianceStamped", description->name);
  EXPECT_EQ("96adb295225031ec8d57fb4251b0a886", description->md5sum);
  EXPECT_EQ("# This represents an estimated accel with reference coordinate frame and timestamp.\nHeader header\nAccelWithCovariance accel\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\n# 0: no frame\n# 1: global frame\nstring frame_id\n\n================================================================================\nMSG: geometry_msgs/AccelWithCovariance\n# This expresses acceleration in free space with uncertainty.\n\nAccel accel\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Accel\n# This expresses acceleration in free space broken into its linear and angular parts.\nVector3  linear\nVector3  angular\n\n================================================================================\nMSG: geometry_msgs/Vector3\n# This represents a vector in free space. \n\nfloat64 x\nfloat64 y\nfloat64 z\n", description->full_text);

  ASSERT_EQ(2, description->fields.size());
  EXPECT_VALUE_FIELD("header", "std_msgs/Header", description->fields[0]);
  EXPECT_VALUE_FIELD("accel", "geometry_msgs/AccelWithCovariance", description->fields[1]);
}

// Tests variable length array
TEST_F(MessageDescriptionProviderTest, get_geometry_msgs_PoseArray_description)
{
  MessageDescription::Ptr description = provider->getDescription("geometry_msgs/PoseArray");
  ASSERT_TRUE(description);
  EXPECT_EQ("geometry_msgs/PoseArray", description->name);
  EXPECT_EQ("916c28c5764443f268b296bb671b9d97", description->md5sum);
  EXPECT_EQ("# An array of poses with a header for global reference.\n\nHeader header\n\nPose[] poses\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\n# 0: no frame\n# 1: global frame\nstring frame_id\n\n================================================================================\nMSG: geometry_msgs/Pose\n# A representation of pose in free space, composed of postion and orientation. \nPoint position\nQuaternion orientation\n\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x\nfloat64 y\nfloat64 z\nfloat64 w\n\n", description->full_text);

  ASSERT_EQ(2, description->fields.size());
  EXPECT_VALUE_FIELD("header", "std_msgs/Header", description->fields[0]);
  EXPECT_VARIABLE_LENGTH_ARRAY_FIELD("poses", "geometry_msgs/Pose", description->fields[1]);
}

int main(int argc, char **argv){
  testing::InitGoogleTest(&argc, argv);

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

  return RUN_ALL_TESTS();
}
<commit_msg>Switched from using accel messages to pose because accel didn't exist in hydro<commit_after>#include <gtest/gtest.h>
#include <ros/ros.h>
#include <boost/scoped_ptr.hpp>
#include <roscpp_message_reflection/message_description_provider.h>

using namespace roscpp_message_reflection;

#define EXPECT_VALUE_FIELD(expected_name, expected_type, field)	\
  EXPECT_EQ(expected_name, field.name());			\
  EXPECT_EQ(expected_type, field.full_type());			\
  EXPECT_EQ(expected_type, field.value_type());			\
  EXPECT_EQ(FieldDescription::Value, field.type())

#define EXPECT_VARIABLE_LENGTH_ARRAY_FIELD(expected_name, expected_value_type, field) \
  EXPECT_EQ(expected_name, field.name());				\
  {									\
    std::stringstream expected_full_type_ss;				\
    expected_full_type_ss << expected_value_type;			\
    expected_full_type_ss << "[]";					\
    EXPECT_EQ(expected_full_type_ss.str(), field.full_type());		\
  }									\
  EXPECT_EQ(expected_value_type, field.value_type());			\
  EXPECT_EQ(FieldDescription::VariableLengthArray, field.type());	\

#define EXPECT_FIXED_LENGTH_ARRAY_FIELD(expected_name, expected_value_type, expected_length, field) \
  EXPECT_EQ(expected_name, field.name());				\
  {									\
    std::stringstream expected_full_type_ss;				\
    expected_full_type_ss << expected_value_type;			\
    expected_full_type_ss << "[" << expected_length << "]";		\
    EXPECT_EQ(expected_full_type_ss.str(), field.full_type());		\
  }									\
  EXPECT_EQ(expected_value_type, field.value_type());			\
  EXPECT_EQ(FieldDescription::FixedLengthArray, field.type());		\
  EXPECT_EQ(expected_length, field.array_length())

TEST(MessageDescription, construct_value_field)
{
  FieldDescription field = FieldDescription::CreateFromFullType("some_name", "AType");
  EXPECT_VALUE_FIELD("some_name", "AType", field);
}

TEST(MessageDescription, construct_variable_length_array_field)
{
  FieldDescription field = FieldDescription::CreateFromFullType("some_name", "AType[]");
  EXPECT_VARIABLE_LENGTH_ARRAY_FIELD("some_name", "AType", field);
}

TEST(MessageDescription, construct_fixed_length_array_field)
{
  FieldDescription field = FieldDescription::CreateFromFullType("some_name", "AType[10]");
  EXPECT_FIXED_LENGTH_ARRAY_FIELD("some_name", "AType", 10, field);
}

TEST(MessageDescription, construct_simple_message)
{
  std::vector<FieldDescription> fields;
  fields.push_back(FieldDescription::CreateFromFullType("a", "int32"));
  fields.push_back(FieldDescription::CreateFromFullType("b", "uint16"));
  fields.push_back(FieldDescription::CreateFromFullType("d", "string"));
  MessageDescription message("a_package/AMessage", "992ce8a1687cec8c8bd883ec73ca1234", "int32 a\nuint16 b\nstring d\n\n", fields, std::map<std::string, MessageDescription::Ptr>());
  EXPECT_EQ("a_package/AMessage", message.name);
  EXPECT_EQ("992ce8a1687cec8c8bd883ec73ca1234", message.md5sum);
  EXPECT_EQ("int32 a\nuint16 b\nstring d\n\n", message.full_text);
  ASSERT_EQ(3, message.fields.size());
  EXPECT_VALUE_FIELD("a", "int32", message.fields[0]);
  EXPECT_VALUE_FIELD("b", "uint16", message.fields[1]);
  EXPECT_VALUE_FIELD("d", "string", message.fields[2]);
}

class MessageDescriptionProviderTest : public ::testing::Test {
protected:
  virtual void SetUp() {
    provider.reset(MessageDescriptionProvider::Create(nh));
  }
  ros::NodeHandle nh;
  boost::scoped_ptr<MessageDescriptionProvider> provider;
};

TEST_F(MessageDescriptionProviderTest, get_std_msgs_String_description)
{
  MessageDescription::Ptr description = provider->getDescription("std_msgs/String");
  ASSERT_TRUE(description);
  EXPECT_EQ("std_msgs/String", description->name);
  EXPECT_EQ("992ce8a1687cec8c8bd883ec73ca41d1", description->md5sum);
  EXPECT_EQ("string data\n\n", description->full_text);

  ASSERT_EQ(1, description->fields.size());
  EXPECT_VALUE_FIELD("data", "string", description->fields[0]);
}

TEST_F(MessageDescriptionProviderTest, get_geometry_msgs_Vector3_description)
{
  MessageDescription::Ptr description = provider->getDescription("geometry_msgs/Vector3");
  ASSERT_TRUE(description);
  EXPECT_EQ("geometry_msgs/Vector3", description->name);
  EXPECT_EQ("4a842b65f413084dc2b10fb484ea7f17", description->md5sum);
  EXPECT_EQ("# This represents a vector in free space. \n\nfloat64 x\nfloat64 y\nfloat64 z\n", description->full_text);

  ASSERT_EQ(3, description->fields.size());
  EXPECT_VALUE_FIELD("x", "float64", description->fields[0]);
  EXPECT_VALUE_FIELD("y", "float64", description->fields[1]);
  EXPECT_VALUE_FIELD("z", "float64", description->fields[2]);
}

// Tests expansion of type without package and fixed length array
TEST_F(MessageDescriptionProviderTest, get_geometry_msgs_PoseWithCovariance_description)
{
  MessageDescription::Ptr description = provider->getDescription("geometry_msgs/PoseWithCovariance");
  ASSERT_TRUE(description);
  EXPECT_EQ("geometry_msgs/PoseWithCovariance", description->name);
  EXPECT_EQ("c23e848cf1b7533a8d7c259073a97e6f", description->md5sum);
  EXPECT_EQ("# This represents a pose in free space with uncertainty.\n\nPose pose\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Pose\n# A representation of pose in free space, composed of postion and orientation. \nPoint position\nQuaternion orientation\n\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x\nfloat64 y\nfloat64 z\nfloat64 w\n\n", description->full_text);

  ASSERT_EQ(2, description->fields.size());
  EXPECT_VALUE_FIELD("pose", "geometry_msgs/Pose", description->fields[0]);
  EXPECT_FIXED_LENGTH_ARRAY_FIELD("covariance", "float64", 36, description->fields[1]);
}

// Tests expansion of type without package recursivly
TEST_F(MessageDescriptionProviderTest, get_geometry_msgs_PoseWithCovarianceStamped_description)
{
  MessageDescription::Ptr description = provider->getDescription("geometry_msgs/PoseWithCovarianceStamped");
  ASSERT_TRUE(description);
  EXPECT_EQ("geometry_msgs/PoseWithCovarianceStamped", description->name);
  EXPECT_EQ("953b798c0f514ff060a53a3498ce6246", description->md5sum);
  EXPECT_EQ("# This expresses an estimated pose with a reference coordinate frame and timestamp\n\nHeader header\nPoseWithCovariance pose\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\n# 0: no frame\n# 1: global frame\nstring frame_id\n\n================================================================================\nMSG: geometry_msgs/PoseWithCovariance\n# This represents a pose in free space with uncertainty.\n\nPose pose\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Pose\n# A representation of pose in free space, composed of postion and orientation. \nPoint position\nQuaternion orientation\n\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x\nfloat64 y\nfloat64 z\nfloat64 w\n\n", description->full_text);

  ASSERT_EQ(2, description->fields.size());
  EXPECT_VALUE_FIELD("header", "std_msgs/Header", description->fields[0]);
  EXPECT_VALUE_FIELD("pose", "geometry_msgs/PoseWithCovariance", description->fields[1]);
}

// Tests variable length array
TEST_F(MessageDescriptionProviderTest, get_geometry_msgs_PoseArray_description)
{
  MessageDescription::Ptr description = provider->getDescription("geometry_msgs/PoseArray");
  ASSERT_TRUE(description);
  EXPECT_EQ("geometry_msgs/PoseArray", description->name);
  EXPECT_EQ("916c28c5764443f268b296bb671b9d97", description->md5sum);
  EXPECT_EQ("# An array of poses with a header for global reference.\n\nHeader header\n\nPose[] poses\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\n# 0: no frame\n# 1: global frame\nstring frame_id\n\n================================================================================\nMSG: geometry_msgs/Pose\n# A representation of pose in free space, composed of postion and orientation. \nPoint position\nQuaternion orientation\n\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x\nfloat64 y\nfloat64 z\nfloat64 w\n\n", description->full_text);

  ASSERT_EQ(2, description->fields.size());
  EXPECT_VALUE_FIELD("header", "std_msgs/Header", description->fields[0]);
  EXPECT_VARIABLE_LENGTH_ARRAY_FIELD("poses", "geometry_msgs/Pose", description->fields[1]);
}

int main(int argc, char **argv){
  testing::InitGoogleTest(&argc, argv);

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

  return RUN_ALL_TESTS();
}
<|endoftext|>


<commit_before>/*=====================================================================

PIXHAWK Micro Air Vehicle Flying Robotics Toolkit

(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>

This file is part of the PIXHAWK project

    PIXHAWK is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    PIXHAWK is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Waypoint list widget
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *   @author Benjamin Knecht <mavteam@student.ethz.ch>
 *   @author Petri Tanskanen <mavteam@student.ethz.ch>
 *
 */

#include "WaypointList.h"
#include "ui_WaypointList.h"
#include <UASInterface.h>
#include <UASManager.h>
#include <QDebug>
#include <QFileDialog>

WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
        QWidget(parent),
        uas(NULL),
        mavX(0.0),
        mavY(0.0),
        mavZ(0.0),
        mavYaw(0.0),
        m_ui(new Ui::WaypointList)
{
    m_ui->setupUi(this);

    listLayout = new QVBoxLayout(m_ui->listWidget);
    listLayout->setSpacing(6);
    listLayout->setMargin(0);
    listLayout->setAlignment(Qt::AlignTop);
    m_ui->listWidget->setLayout(listLayout);

    this->uas = NULL;

    // ADD WAYPOINT
    // Connect add action, set right button icon and connect action to this class
    connect(m_ui->addButton, SIGNAL(clicked()), m_ui->actionAddWaypoint, SIGNAL(triggered()));
    connect(m_ui->actionAddWaypoint, SIGNAL(triggered()), this, SLOT(add()));

    // ADD WAYPOINT AT CURRENT POSITION
    connect(m_ui->positionAddButton, SIGNAL(clicked()), this, SLOT(addCurrentPositonWaypoint()));

    // SEND WAYPOINTS
    connect(m_ui->transmitButton, SIGNAL(clicked()), this, SLOT(transmit()));

    // REQUEST WAYPOINTS
    connect(m_ui->readButton, SIGNAL(clicked()), this, SLOT(read()));

    // SAVE/LOAD WAYPOINTS
    connect(m_ui->saveButton, SIGNAL(clicked()), this, SLOT(saveWaypoints()));
    connect(m_ui->loadButton, SIGNAL(clicked()), this, SLOT(loadWaypoints()));

    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));

    // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED
    setUAS(uas);

    // STATUS LABEL
    updateStatusLabel("");

    this->setVisible(false);
}

WaypointList::~WaypointList()
{
    delete m_ui;
}

void WaypointList::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec)
{
    Q_UNUSED(uas);
    Q_UNUSED(usec);
    mavX = x;
    mavY = y;
    mavZ = z;
}

void WaypointList::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec)
{
    Q_UNUSED(uas);
    Q_UNUSED(usec);
    Q_UNUSED(roll);
    Q_UNUSED(pitch);
    mavYaw = yaw;
}

void WaypointList::setUAS(UASInterface* uas)
{
    if (this->uas == NULL && uas != NULL)
    {
        this->uas = uas;

        connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)),  this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
        connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)),       this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));

        connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)),        this, SLOT(updateStatusLabel(const QString &)));
        connect(&uas->getWaypointManager(), SIGNAL(waypointListChanged(void)),                  this, SLOT(waypointListChanged(void)));
        connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)),            this, SLOT(currentWaypointChanged(quint16)));
    }
}

void WaypointList::saveWaypoints()
{
    if (uas)
    {
        QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)"));
        uas->getWaypointManager().localSaveWaypoints(fileName);
    }
}

void WaypointList::loadWaypoints()
{
    if (uas)
    {
        QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)"));
        uas->getWaypointManager().localLoadWaypoints(fileName);
    }
}

void WaypointList::transmit()
{
    if (uas)
    {
        uas->getWaypointManager().writeWaypoints();
    }
}

void WaypointList::read()
{
    if (uas)
    {
        uas->getWaypointManager().readWaypoints();
    }
}

void WaypointList::add()
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
        if (waypoints.size() > 0)
        {
            Waypoint *last = waypoints.at(waypoints.size()-1);
            Waypoint *wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
            uas->getWaypointManager().localAddWaypoint(wp);
        }
        else
        {
            Waypoint *wp = new Waypoint(0, 1.1, 1.1, -0.8, 0.0, true, true, 0.15, 2000);
            uas->getWaypointManager().localAddWaypoint(wp);
        }
    }
}

void WaypointList::addCurrentPositonWaypoint()
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
        if (waypoints.size() > 0)
        {
            Waypoint *last = waypoints.at(waypoints.size()-1);
            Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
            uas->getWaypointManager().localAddWaypoint(wp);
        }
        else
        {
            Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000);
            uas->getWaypointManager().localAddWaypoint(wp);
        }
    }
}

void WaypointList::updateStatusLabel(const QString &string)
{
    m_ui->statusLabel->setText(string);
}

void WaypointList::changeCurrentWaypoint(quint16 seq)
{
    if (uas)
    {
        uas->getWaypointManager().setCurrentWaypoint(seq);
    }
}

void WaypointList::currentWaypointChanged(quint16 seq)
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();

        if (seq < waypoints.size())
        {
            for(int i = 0; i < waypoints.size(); i++)
            {
                WaypointView* widget = wpViews.find(waypoints[i]).value();

                if (waypoints[i]->getId() == seq)
                {
                    widget->setCurrent(true);
                }
                else
                {
                    widget->setCurrent(false);
                }
            }
        }
    }
}

void WaypointList::waypointListChanged()
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();

        // first remove all views of non existing waypoints
        if (!wpViews.empty())
        {
            QMapIterator<Waypoint*,WaypointView*> viewIt(wpViews);
            viewIt.toFront();
            while(viewIt.hasNext())
            {
                viewIt.next();
                Waypoint *cur = viewIt.key();
                int i;
                for (i = 0; i < waypoints.size(); i++)
                {
                    if (waypoints[i] == cur)
                    {
                        break;
                    }
                }
                if (i == waypoints.size())
                {
                    WaypointView* widget = wpViews.find(cur).value();
                    widget->hide();
                    listLayout->removeWidget(widget);
                    wpViews.remove(cur);
                }
            }
        }

        // then add/update the views for each waypoint in the list
        for(int i = 0; i < waypoints.size(); i++)
        {
            Waypoint *wp = waypoints[i];
            if (!wpViews.contains(wp))
            {
                WaypointView* wpview = new WaypointView(wp, this);
                wpViews.insert(wp, wpview);
                connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)),    this, SLOT(moveDown(Waypoint*)));
                connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)),      this, SLOT(moveUp(Waypoint*)));
                connect(wpview, SIGNAL(removeWaypoint(Waypoint*)),      this, SLOT(removeWaypoint(Waypoint*)));
                connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
                connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
            }

            WaypointView *wpv = wpViews.value(wp);
            wpv->updateValues();    // update the values of the ui elements in the view
            listLayout->addWidget(wpv);
        }
    }
}

void WaypointList::moveUp(Waypoint* wp)
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();

        //get the current position of wp in the local storage
        int i;
        for (i = 0; i < waypoints.size(); i++)
        {
            if (waypoints[i] == wp)
                break;
        }

        // if wp was found and its not the first entry, move it
        if (i < waypoints.size() && i > 0)
        {
            uas->getWaypointManager().localMoveWaypoint(i, i-1);
        }
    }
}

void WaypointList::moveDown(Waypoint* wp)
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();

        //get the current position of wp in the local storage
        int i;
        for (i = 0; i < waypoints.size(); i++)
        {
            if (waypoints[i] == wp)
                break;
        }

        // if wp was found and its not the last entry, move it
        if (i < waypoints.size()-1)
        {
            uas->getWaypointManager().localMoveWaypoint(i, i+1);
        }
    }
}

void WaypointList::removeWaypoint(Waypoint* wp)
{
    if (uas)
    {
        uas->getWaypointManager().localRemoveWaypoint(wp->getId());
    }
}

void WaypointList::changeEvent(QEvent *e)
{
    switch (e->type()) {
    case QEvent::LanguageChange:
        m_ui->retranslateUi(this);
        break;
    default:
        break;
    }
}

void WaypointList::on_clearWPListButton_clicked()
{
    emit clearPathclicked();

//    if (uas)
//    {
//        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
//            for(int i = 0; i <=waypoints.size(); i++)
//            {
//                WaypointView* widget = wpViews.find(waypoints[i]).value();

//                widget->remove();

//            }

//    }
}

/** @brief Add a waypoint by mouse click over the map */
void WaypointList::addWaypointMouse(QPointF coordinate)
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
        if (waypoints.size() > 0)
        {
            Waypoint *last = waypoints.at(waypoints.size()-1);
            Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
            uas->getWaypointManager().localAddWaypoint(wp);
        }
        else
        {
            Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), -0.8, 0.0, true, true, 0.15, 2000);
            uas->getWaypointManager().localAddWaypoint(wp);
        }
    }

}
<commit_msg>add the functionality to erase the list of Waypoint widget (WaypointView) from the button of the WaypointList widget<commit_after>/*=====================================================================

PIXHAWK Micro Air Vehicle Flying Robotics Toolkit

(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>

This file is part of the PIXHAWK project

    PIXHAWK is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    PIXHAWK is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Waypoint list widget
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *   @author Benjamin Knecht <mavteam@student.ethz.ch>
 *   @author Petri Tanskanen <mavteam@student.ethz.ch>
 *
 */

#include "WaypointList.h"
#include "ui_WaypointList.h"
#include <UASInterface.h>
#include <UASManager.h>
#include <QDebug>
#include <QFileDialog>

WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
        QWidget(parent),
        uas(NULL),
        mavX(0.0),
        mavY(0.0),
        mavZ(0.0),
        mavYaw(0.0),
        m_ui(new Ui::WaypointList)
{
    m_ui->setupUi(this);

    listLayout = new QVBoxLayout(m_ui->listWidget);
    listLayout->setSpacing(6);
    listLayout->setMargin(0);
    listLayout->setAlignment(Qt::AlignTop);
    m_ui->listWidget->setLayout(listLayout);

    this->uas = NULL;

    // ADD WAYPOINT
    // Connect add action, set right button icon and connect action to this class
    connect(m_ui->addButton, SIGNAL(clicked()), m_ui->actionAddWaypoint, SIGNAL(triggered()));
    connect(m_ui->actionAddWaypoint, SIGNAL(triggered()), this, SLOT(add()));

    // ADD WAYPOINT AT CURRENT POSITION
    connect(m_ui->positionAddButton, SIGNAL(clicked()), this, SLOT(addCurrentPositonWaypoint()));

    // SEND WAYPOINTS
    connect(m_ui->transmitButton, SIGNAL(clicked()), this, SLOT(transmit()));

    // REQUEST WAYPOINTS
    connect(m_ui->readButton, SIGNAL(clicked()), this, SLOT(read()));

    // SAVE/LOAD WAYPOINTS
    connect(m_ui->saveButton, SIGNAL(clicked()), this, SLOT(saveWaypoints()));
    connect(m_ui->loadButton, SIGNAL(clicked()), this, SLOT(loadWaypoints()));

    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));

    // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED
    setUAS(uas);

    // STATUS LABEL
    updateStatusLabel("");

    this->setVisible(false);
}

WaypointList::~WaypointList()
{
    delete m_ui;
}

void WaypointList::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec)
{
    Q_UNUSED(uas);
    Q_UNUSED(usec);
    mavX = x;
    mavY = y;
    mavZ = z;
}

void WaypointList::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec)
{
    Q_UNUSED(uas);
    Q_UNUSED(usec);
    Q_UNUSED(roll);
    Q_UNUSED(pitch);
    mavYaw = yaw;
}

void WaypointList::setUAS(UASInterface* uas)
{
    if (this->uas == NULL && uas != NULL)
    {
        this->uas = uas;

        connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)),  this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
        connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)),       this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));

        connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)),        this, SLOT(updateStatusLabel(const QString &)));
        connect(&uas->getWaypointManager(), SIGNAL(waypointListChanged(void)),                  this, SLOT(waypointListChanged(void)));
        connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)),            this, SLOT(currentWaypointChanged(quint16)));
    }
}

void WaypointList::saveWaypoints()
{
    if (uas)
    {
        QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)"));
        uas->getWaypointManager().localSaveWaypoints(fileName);
    }
}

void WaypointList::loadWaypoints()
{
    if (uas)
    {
        QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)"));
        uas->getWaypointManager().localLoadWaypoints(fileName);
    }
}

void WaypointList::transmit()
{
    if (uas)
    {
        uas->getWaypointManager().writeWaypoints();
    }
}

void WaypointList::read()
{
    if (uas)
    {
        uas->getWaypointManager().readWaypoints();
    }
}

void WaypointList::add()
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
        if (waypoints.size() > 0)
        {
            Waypoint *last = waypoints.at(waypoints.size()-1);
            Waypoint *wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
            uas->getWaypointManager().localAddWaypoint(wp);
        }
        else
        {
            Waypoint *wp = new Waypoint(0, 1.1, 1.1, -0.8, 0.0, true, true, 0.15, 2000);
            uas->getWaypointManager().localAddWaypoint(wp);
        }
    }
}

void WaypointList::addCurrentPositonWaypoint()
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
        if (waypoints.size() > 0)
        {
            Waypoint *last = waypoints.at(waypoints.size()-1);
            Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
            uas->getWaypointManager().localAddWaypoint(wp);
        }
        else
        {
            Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000);
            uas->getWaypointManager().localAddWaypoint(wp);
        }
    }
}

void WaypointList::updateStatusLabel(const QString &string)
{
    m_ui->statusLabel->setText(string);
}

void WaypointList::changeCurrentWaypoint(quint16 seq)
{
    if (uas)
    {
        uas->getWaypointManager().setCurrentWaypoint(seq);
    }
}

void WaypointList::currentWaypointChanged(quint16 seq)
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();

        if (seq < waypoints.size())
        {
            for(int i = 0; i < waypoints.size(); i++)
            {
                WaypointView* widget = wpViews.find(waypoints[i]).value();

                if (waypoints[i]->getId() == seq)
                {
                    widget->setCurrent(true);
                }
                else
                {
                    widget->setCurrent(false);
                }
            }
        }
    }
}

void WaypointList::waypointListChanged()
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();

        // first remove all views of non existing waypoints
        if (!wpViews.empty())
        {
            QMapIterator<Waypoint*,WaypointView*> viewIt(wpViews);
            viewIt.toFront();
            while(viewIt.hasNext())
            {
                viewIt.next();
                Waypoint *cur = viewIt.key();
                int i;
                for (i = 0; i < waypoints.size(); i++)
                {
                    if (waypoints[i] == cur)
                    {
                        break;
                    }
                }
                if (i == waypoints.size())
                {
                    WaypointView* widget = wpViews.find(cur).value();
                    widget->hide();
                    listLayout->removeWidget(widget);
                    wpViews.remove(cur);
                }
            }
        }

        // then add/update the views for each waypoint in the list
        for(int i = 0; i < waypoints.size(); i++)
        {
            Waypoint *wp = waypoints[i];
            if (!wpViews.contains(wp))
            {
                WaypointView* wpview = new WaypointView(wp, this);
                wpViews.insert(wp, wpview);
                connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)),    this, SLOT(moveDown(Waypoint*)));
                connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)),      this, SLOT(moveUp(Waypoint*)));
                connect(wpview, SIGNAL(removeWaypoint(Waypoint*)),      this, SLOT(removeWaypoint(Waypoint*)));
                connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
                connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16)));
            }

            WaypointView *wpv = wpViews.value(wp);
            wpv->updateValues();    // update the values of the ui elements in the view
            listLayout->addWidget(wpv);
        }
    }
}

void WaypointList::moveUp(Waypoint* wp)
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();

        //get the current position of wp in the local storage
        int i;
        for (i = 0; i < waypoints.size(); i++)
        {
            if (waypoints[i] == wp)
                break;
        }

        // if wp was found and its not the first entry, move it
        if (i < waypoints.size() && i > 0)
        {
            uas->getWaypointManager().localMoveWaypoint(i, i-1);
        }
    }
}

void WaypointList::moveDown(Waypoint* wp)
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();

        //get the current position of wp in the local storage
        int i;
        for (i = 0; i < waypoints.size(); i++)
        {
            if (waypoints[i] == wp)
                break;
        }

        // if wp was found and its not the last entry, move it
        if (i < waypoints.size()-1)
        {
            uas->getWaypointManager().localMoveWaypoint(i, i+1);
        }
    }
}

void WaypointList::removeWaypoint(Waypoint* wp)
{
    if (uas)
    {
        uas->getWaypointManager().localRemoveWaypoint(wp->getId());
    }
}

void WaypointList::changeEvent(QEvent *e)
{
    switch (e->type()) {
    case QEvent::LanguageChange:
        m_ui->retranslateUi(this);
        break;
    default:
        break;
    }
}

void WaypointList::on_clearWPListButton_clicked()
{
    emit clearPathclicked();

    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
            while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++)
            {
                //Waypoint *temp = waypoints[i];

                WaypointView* widget = wpViews.find(waypoints[0]).value();

                widget->remove();

            }

    }
}

/** @brief Add a waypoint by mouse click over the map */
void WaypointList::addWaypointMouse(QPointF coordinate)
{
    if (uas)
    {
        const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
        if (waypoints.size() > 0)
        {
            Waypoint *last = waypoints.at(waypoints.size()-1);
            Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
            uas->getWaypointManager().localAddWaypoint(wp);
        }
        else
        {
            Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), -0.8, 0.0, true, true, 0.15, 2000);
            uas->getWaypointManager().localAddWaypoint(wp);
        }
    }

}
<|endoftext|>