Skip to content

Commit fdedd8f

Browse files
delta-12delta-12
committed
CVW-77 waypoint message
1 parent 6c9df59 commit fdedd8f

6 files changed

Lines changed: 123 additions & 0 deletions

File tree

‎lib/c++/inc/cave_talk.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#include "odometry.pb.h"
1717
#include "ooga_booga.pb.h"
1818
#include "relative_move.pb.h"
19+
#include "waypoint.pb.h"
1920

2021
#include "cave_talk_link.h"
2122
#include "cave_talk_types.h"
@@ -43,6 +44,7 @@ class ListenerCallbacks
4344
virtual void HearConfigSteeringControl(const PID &turn_rate_params, const bool enabled) = 0;
4445
virtual void HearAirQuality(const uint32_t dust_ug_per_m3, const uint32_t gas_ppm, const double temperature_celsius) = 0;
4546
virtual void HearRelativeMove(const RelativeMoveType type, const CaveTalk_Meter_t position, const CaveTalk_Radian_t pose) = 0;
47+
virtual void HearWaypoint(const WaypointType type, const CaveTalk_Meter_t x, const CaveTalk_Meter_t y, const CaveTalk_Radian_t heading) = 0;
4648
};
4749

4850
class Listener
@@ -73,6 +75,7 @@ class Listener
7375
CaveTalk_Error_t HandleConfigSteeringControl(const CaveTalk_Length_t length) const;
7476
CaveTalk_Error_t HandleAirQuality(const CaveTalk_Length_t length) const;
7577
CaveTalk_Error_t HandleRelativeMove(CaveTalk_Length_t length) const;
78+
CaveTalk_Error_t HandleWaypoint(CaveTalk_Length_t length) const;
7679
CaveTalk_LinkHandle_t link_handle_;
7780
std::shared_ptr<ListenerCallbacks> listener_callbacks_;
7881
std::array<uint8_t, CAVE_TALK_MAX_PAYLOAD_SIZE> buffer_;
@@ -102,6 +105,7 @@ class Talker
102105
CaveTalk_Error_t SpeakConfigSteeringControl(const PID &turn_rate_params, const bool enabled);
103106
CaveTalk_Error_t SpeakAirQuality(const uint32_t dust_ug_per_m3, const uint32_t gas_ppm, const double temperature_celsius);
104107
CaveTalk_Error_t SpeakRelativeMove(const RelativeMoveType type, const CaveTalk_Meter_t position, const CaveTalk_Radian_t pose);
108+
CaveTalk_Error_t SpeakWaypoint(const WaypointType type, const CaveTalk_Meter_t x, const CaveTalk_Meter_t y, const CaveTalk_Radian_t heading);
105109

106110
private:
107111
CaveTalk_LinkHandle_t link_handle_;

‎lib/c++/src/cave_talk.cc‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include "odometry.pb.h"
2121
#include "ooga_booga.pb.h"
2222
#include "relative_move.pb.h"
23+
#include "waypoint.pb.h"
2324

2425
namespace cave_talk
2526
{
@@ -98,6 +99,9 @@ CaveTalk_Error_t Listener::Listen(void)
9899
case ID_RELATIVE_MOVE:
99100
error = HandleRelativeMove(length);
100101
break;
102+
case ID_WAYPOINT:
103+
error = HandleWaypoint(length);
104+
break;
101105
default:
102106
error = CAVE_TALK_ERROR_ID;
103107
break;
@@ -389,6 +393,25 @@ CaveTalk_Error_t Listener::HandleRelativeMove(CaveTalk_Length_t length) const
389393
return CAVE_TALK_ERROR_NONE;
390394
}
391395

396+
CaveTalk_Error_t Listener::HandleWaypoint(CaveTalk_Length_t length) const
397+
{
398+
Waypoint waypoint_message;
399+
400+
if (!waypoint_message.ParseFromArray(buffer_.data(), length))
401+
{
402+
return CAVE_TALK_ERROR_PARSE;
403+
}
404+
405+
const WaypointType type = waypoint_message.type();
406+
const CaveTalk_Meter_t x = waypoint_message.x_meters();
407+
const CaveTalk_Meter_t y = waypoint_message.y_meters();
408+
const CaveTalk_Radian_t heading = waypoint_message.heading_radians();
409+
410+
listener_callbacks_->HearWaypoint(type, x, y, heading);
411+
412+
return CAVE_TALK_ERROR_NONE;
413+
}
414+
392415
Talker::Talker(CaveTalk_Error_t (*send)(const void *const data, const size_t size))
393416
{
394417
link_handle_ = kCaveTalk_LinkHandleNull;
@@ -605,4 +628,18 @@ CaveTalk_Error_t Talker::SpeakRelativeMove(const RelativeMoveType type, const Ca
605628
return CaveTalk_Speak(&link_handle_, static_cast<CaveTalk_Id_t>(ID_RELATIVE_MOVE), message_buffer_.data(), length);
606629
}
607630

631+
CaveTalk_Error_t Talker::SpeakWaypoint(const WaypointType type, const CaveTalk_Meter_t x, const CaveTalk_Meter_t y, const CaveTalk_Radian_t heading)
632+
{
633+
Waypoint waypoint_message;
634+
waypoint_message.set_type(type);
635+
waypoint_message.set_x_meters(x);
636+
waypoint_message.set_y_meters(y);
637+
waypoint_message.set_heading_radians(heading);
638+
639+
std::size_t length = waypoint_message.ByteSizeLong();
640+
waypoint_message.SerializeToArray(message_buffer_.data(), message_buffer_.max_size());
641+
642+
return CaveTalk_Speak(&link_handle_, static_cast<CaveTalk_Id_t>(ID_WAYPOINT), message_buffer_.data(), length);
643+
}
644+
608645
} // namespace cave_talk

‎lib/c/inc/cave_talk.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
#include "config_pid.pb.h"
1414
#include "config_servo.pb.h"
1515
#include "relative_move.pb.h"
16+
#include "waypoint.pb.h"
1617

1718
#include "cave_talk_link.h"
1819
#include "cave_talk_types.h"
@@ -35,6 +36,7 @@ typedef struct
3536
void (*hear_config_steering_control)(const cave_talk_PID *const turn_rate_params, const bool enabled);
3637
void (*hear_air_quality)(const uint32_t dust_ug_per_m3, const uint32_t gas_ppm, const double temperature_celsius);
3738
void (*hear_relative_move)(const cave_talk_RelativeMoveType type, const CaveTalk_Meter_t position, const CaveTalk_Radian_t pose);
39+
void (*hear_waypoint)(const cave_talk_WaypointType type, const CaveTalk_Meter_t x, const CaveTalk_Meter_t y, const CaveTalk_Radian_t heading);
3840
} CaveTalk_ListenCallbacks_t;
3941

4042
typedef struct
@@ -62,6 +64,7 @@ static const CaveTalk_ListenCallbacks_t kCaveTalk_ListenCallbacksNull = {
6264
.hear_config_steering_control = NULL,
6365
.hear_air_quality = NULL,
6466
.hear_relative_move = NULL,
67+
.hear_waypoint = NULL,
6568
};
6669

6770
static const CaveTalk_Handle_t kCaveTalk_HandleNull = {
@@ -94,6 +97,7 @@ CaveTalk_Error_t CaveTalk_SpeakConfigWheelSpeedControl(const CaveTalk_Handle_t *
9497
CaveTalk_Error_t CaveTalk_SpeakConfigSteeringControl(const CaveTalk_Handle_t *const handle, const cave_talk_PID *const turn_rate_params, const bool enabled);
9598
CaveTalk_Error_t CaveTalk_SpeakAirQuality(const CaveTalk_Handle_t *const handle, const uint32_t dust_ug_per_m3, const uint32_t gas_ppm, const double temperature_celsius);
9699
CaveTalk_Error_t CaveTalk_SpeakRelativeMove(const CaveTalk_Handle_t *const handle, const cave_talk_RelativeMoveType type, const CaveTalk_Meter_t position, const CaveTalk_Radian_t pose);
100+
CaveTalk_Error_t CaveTalk_SpeakWaypoint(const CaveTalk_Handle_t *const handle, const cave_talk_WaypointType type, const CaveTalk_Meter_t x, const CaveTalk_Meter_t y, const CaveTalk_Radian_t heading);
97101

98102
#ifdef __cplusplus
99103
}

‎lib/c/src/cave_talk.c‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#include "ooga_booga.pb.h"
1919
#include "pb_decode.h"
2020
#include "pb_encode.h"
21+
#include "waypoint.pb.h"
2122

2223
#include "cave_talk_link.h"
2324
#include "cave_talk_types.h"
@@ -38,6 +39,7 @@ static CaveTalk_Error_t CaveTalk_HandleConfigWheelSpeedControl(const CaveTalk_Ha
3839
static CaveTalk_Error_t CaveTalk_HandleConfigSteeringControl(const CaveTalk_Handle_t *const handle, const CaveTalk_Length_t length);
3940
static CaveTalk_Error_t CaveTalk_HandleAirQuality(const CaveTalk_Handle_t *const handle, const CaveTalk_Length_t length);
4041
static CaveTalk_Error_t CaveTalk_HandleRelativeMove(const CaveTalk_Handle_t *const handle, const CaveTalk_Length_t length);
42+
static CaveTalk_Error_t CaveTalk_HandleWaypoint(const CaveTalk_Handle_t *const handle, const CaveTalk_Length_t length);
4143
static bool CaveTalk_EncodeString(pb_ostream_t *stream, const pb_field_t *field, void *const *arg);
4244
static bool CaveTalk_DecodeString(pb_istream_t *stream, const pb_field_t *field, void **arg);
4345

@@ -127,6 +129,9 @@ CaveTalk_Error_t CaveTalk_Hear(CaveTalk_Handle_t *const handle)
127129
case cave_talk_Id_ID_RELATIVE_MOVE:
128130
error = CaveTalk_HandleRelativeMove(handle, length);
129131
break;
132+
case cave_talk_Id_ID_WAYPOINT:
133+
error = CaveTalk_HandleWaypoint(handle, length);
134+
break;
130135
default:
131136
error = CAVE_TALK_ERROR_ID;
132137
break;
@@ -719,6 +724,36 @@ CaveTalk_Error_t CaveTalk_SpeakRelativeMove(const CaveTalk_Handle_t *const handl
719724
return error;
720725
}
721726

727+
CaveTalk_Error_t CaveTalk_SpeakWaypoint(const CaveTalk_Handle_t *const handle, const cave_talk_WaypointType type, const CaveTalk_Meter_t x, const CaveTalk_Meter_t y, const CaveTalk_Radian_t heading)
728+
{
729+
CaveTalk_Error_t error = CAVE_TALK_ERROR_NULL;
730+
731+
if ((NULL == handle) || (NULL == handle->buffer) || (NULL == handle->link_handle.send))
732+
{
733+
}
734+
else
735+
{
736+
pb_ostream_t ostream = pb_ostream_from_buffer(handle->buffer, handle->buffer_size);
737+
cave_talk_Waypoint waypoint_message = cave_talk_Waypoint_init_zero;
738+
739+
waypoint_message.type = type;
740+
waypoint_message.x_meters = x;
741+
waypoint_message.y_meters = y;
742+
waypoint_message.heading_radians = heading;
743+
744+
if (!pb_encode(&ostream, cave_talk_Waypoint_fields, &waypoint_message))
745+
{
746+
error = CAVE_TALK_ERROR_SIZE;
747+
}
748+
else
749+
{
750+
error = CaveTalk_Speak(&handle->link_handle, (CaveTalk_Id_t)cave_talk_Id_ID_WAYPOINT, handle->buffer, ostream.bytes_written);
751+
}
752+
}
753+
754+
return error;
755+
}
756+
722757
static CaveTalk_Error_t CaveTalk_HandleOogaBooga(const CaveTalk_Handle_t *const handle, const CaveTalk_Length_t length)
723758
{
724759
CaveTalk_Error_t error = CAVE_TALK_ERROR_NONE;
@@ -1139,6 +1174,32 @@ static CaveTalk_Error_t CaveTalk_HandleRelativeMove(const CaveTalk_Handle_t *con
11391174
return error;
11401175
}
11411176

1177+
static CaveTalk_Error_t CaveTalk_HandleWaypoint(const CaveTalk_Handle_t *const handle, const CaveTalk_Length_t length)
1178+
{
1179+
CaveTalk_Error_t error = CAVE_TALK_ERROR_NONE;
1180+
1181+
if ((NULL == handle) || (NULL == handle->buffer))
1182+
{
1183+
error = CAVE_TALK_ERROR_NULL;
1184+
}
1185+
else
1186+
{
1187+
pb_istream_t istream = pb_istream_from_buffer(handle->buffer, length);
1188+
cave_talk_Waypoint waypoint_message = cave_talk_Waypoint_init_zero;
1189+
1190+
if (!pb_decode(&istream, cave_talk_Waypoint_fields, &waypoint_message))
1191+
{
1192+
error = CAVE_TALK_ERROR_PARSE;
1193+
}
1194+
else if (NULL != handle->listen_callbacks.hear_waypoint)
1195+
{
1196+
handle->listen_callbacks.hear_waypoint(waypoint_message.type, waypoint_message.x_meters, waypoint_message.y_meters, waypoint_message.heading_radians);
1197+
}
1198+
}
1199+
1200+
return error;
1201+
}
1202+
11421203
static bool CaveTalk_EncodeString(pb_ostream_t *stream, const pb_field_t *field, void *const *arg)
11431204
{
11441205
bool encoded = false;

‎messages/ids.proto‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,4 +20,5 @@ enum Id {
2020
ID_CONFIG_STEERING_CONTROL = 14;
2121
ID_AIR_QUALITY = 15;
2222
ID_RELATIVE_MOVE = 16;
23+
ID_WAYPOINT = 17;
2324
}

‎messages/waypoint.proto‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
syntax = "proto3";
2+
3+
package cave_talk;
4+
5+
enum WaypointType {
6+
WAYPOINT_TYPE_CMD = 0;
7+
WAYPOINT_TYPE_ACK = 1;
8+
WAYPOINT_TYPE_NACK = 2;
9+
}
10+
11+
message Waypoint {
12+
WaypointType type = 1;
13+
double x_meters = 2;
14+
double y_meters = 3;
15+
double heading_radians = 4;
16+
}

0 commit comments

Comments
 (0)