Merge new examples for CPP

examples/cpp/.gitignore

@@ -22,4 +22,7 @@ send_image_datapoint
 send_localization_datapoint
 send_gps_location_datapoint
 send_receive_intervention_request
-send_event
+send_event
+send_receive_custom_data_channel
+teleop
+teleop_heartbeat_callback

examples/cpp/CMakeLists.txt

@@ -61,6 +61,17 @@ target_link_libraries(
   protobuf::libprotobuf
 )
 
+add_executable(
+  send_receive_custom_data_channel 
+  send_receive_custom_data_channel.cpp
+  ${INCLUDES}
+)
+target_link_libraries(
+  send_receive_custom_data_channel 
+  gRPC::grpc++
+  protobuf::libprotobuf
+)
+
 add_executable(
   get_config_params
   get_config_params.cpp
@@ -148,3 +159,25 @@ target_link_libraries(
   gRPC::grpc++
   protobuf::libprotobuf
 )
+
+add_executable(
+  teleop 
+  teleop.cpp
+  ${INCLUDES}
+)
+target_link_libraries(
+  teleop 
+  gRPC::grpc++
+  protobuf::libprotobuf
+)
+
+add_executable(
+  teleop_heartbeat_callback 
+  teleop_heartbeat_callback.cpp
+  ${INCLUDES}
+)
+target_link_libraries(
+  teleop_heartbeat_callback 
+  gRPC::grpc++
+  protobuf::libprotobuf
+)

examples/cpp/send_receive_custom_data_channel.cpp

@@ -0,0 +1,97 @@
+#include <iostream>
+#include <limits.h>
+#include <memory>
+#include <string>
+#include <sys/time.h>
+#include <unistd.h>
+#include <signal.h>
+
+#include <grpcpp/grpcpp.h>
+
+#include "agent/v1/agent.grpc.pb.h"
+
+using grpc::Channel;
+using grpc::ClientContext;
+using grpc::Status;
+
+using v1::agent::Agent;
+using v1::agent::GetCustomDataChannelMessageStreamRequest;
+using v1::agent::GetCustomDataChannelMessageStreamResponse;
+using v1::agent::SendOnCustomDataChannelRequest;
+using v1::agent::SendOnCustomDataChannelResponse;
+
+class FormantAgentClient
+{
+public:
+   FormantAgentClient(std::shared_ptr<Channel> channel) : stub_(Agent::NewStub(channel)) {}
+
+   void SendReceiveOnCustomDataChannel()
+   {
+      std::cout << "beginning to listen for data..." << std::endl;
+
+      // Set up the stream
+      ClientContext context;
+      GetCustomDataChannelMessageStreamRequest request;
+      auto stream = stub_->GetCustomDataChannelMessageStream(&context, request);
+
+      // Handle an infinite blocking list of commands
+      GetCustomDataChannelMessageStreamResponse message;
+      while (stream->Read(&message))
+      {
+         auto peer_id = message.peer_id();
+         auto channel_name = message.channel_name();
+         auto payload = message.payload();
+
+         // Print the peer id
+         std::cout << "Peer id: " << peer_id << std::endl;
+
+         // Print the channel name
+         std::cout << "Channel: " << channel_name << std::endl;
+
+         // Print the payload
+         std::cout << "Payload: " << payload << std::endl;
+
+         // Handle each command separately
+         if (channel_name == "test-on-channel")
+         {
+            std::cout << "test-sdk channel message with payload: " << payload << std::endl;
+         }
+         else if (channel_name == "joystick")
+         {
+            std::cout << "joystick channel message with payload: " << payload << std::endl;
+         }
+
+         // Set up for the finish call
+         ClientContext finishContext;
+         SendOnCustomDataChannelRequest finishRequest;
+         SendOnCustomDataChannelResponse finishResponse;
+
+         // create json payload
+         auto json = "{\"values\": [1, 2, 3], \"states\": [true, false, true]}";
+
+         // Set the response with the request id and success / failure
+         finishRequest.set_channel_name("test-sdk");
+         finishRequest.set_payload(json);
+
+         // Send the response
+         stub_->SendOnCustomDataChannel(&finishContext, finishRequest, &finishResponse);
+      }
+      Status status = stream->Finish();
+
+         return;
+   }
+
+private:
+   std::unique_ptr<Agent::Stub> stub_;
+};
+
+int main(int argc, char **argv)
+{
+   // Create the Formant Agent client object
+   FormantAgentClient client(grpc::CreateChannel("localhost:5501", grpc::InsecureChannelCredentials()));
+
+   // Start listening for commands
+   client.SendReceiveOnCustomDataChannel();
+
+   return 0;
+}

examples/cpp/teleop.cpp

@@ -0,0 +1,154 @@
+#include <iostream>
+#include <limits.h>
+#include <memory>
+#include <string>
+#include <sys/time.h>
+#include <unistd.h>
+#include <signal.h>
+
+#include <grpcpp/grpcpp.h>
+
+#include "agent/v1/agent.grpc.pb.h"
+
+using grpc::Channel;
+using grpc::ClientContext;
+using grpc::Status;
+
+using v1::agent::Agent;
+using v1::agent::GetTeleopControlDataStreamRequest;
+using v1::agent::GetTeleopControlDataStreamResponse;
+using v1::agent::PostDataResponse;
+using v1::model::Datapoint;
+using v1::model::Bitset;
+
+class FormantAgentClient
+{
+public:
+   FormantAgentClient(std::shared_ptr<Channel> channel) : stub_(Agent::NewStub(channel)) {}
+
+   void Teleop()
+   {
+      std::cout << "beginning to listen for data..." << std::endl;
+
+      // Set up the stream
+      ClientContext context;
+      GetTeleopControlDataStreamRequest request;
+      auto stream = stub_->GetTeleopControlDataStream(&context, request);
+
+      // Handle an infinite blocking list of commands
+      GetTeleopControlDataStreamResponse message;
+      while (stream->Read(&message))
+      {
+         auto stream = message.control_datapoint().stream();
+         auto timestamp = message.control_datapoint().timestamp();
+
+         // Print the stream name and timestamp
+         std::cout << "Stream name: " << stream
+                     <<  " at timestamp: " << timestamp <<std::endl;
+
+         if (stream == "joystick") {
+            // Capture joystick axes
+            auto twist_x = message.control_datapoint().twist().angular().x();
+            auto twist_y = message.control_datapoint().twist().angular().y();
+            auto twist_z = message.control_datapoint().twist().angular().z();
+
+            auto linear_x = message.control_datapoint().twist().linear().x();
+            auto linear_y = message.control_datapoint().twist().linear().y();
+            auto linear_z = message.control_datapoint().twist().linear().z();
+
+            std::cout << "    " 
+                        << "Twist:  " << twist_x << ", " 
+                                    << twist_y << ", " 
+                                    << twist_z << ", " 
+                        << "  Linear: " << linear_x << ", " 
+                                       << linear_y << ", " 
+                                       << linear_z << std::endl;
+
+         } else if (stream == "Buttons") {
+
+            auto button_name = message.control_datapoint().bitset().bits()[0].key();
+            std::cout << "    Button " << button_name << " has been pressed." << std::endl;
+
+         } else if (stream == "Localization") {
+            auto translation_x = message.control_datapoint().pose().translation().x();
+            auto quaternion_w = message.control_datapoint().pose().rotation().w();
+            std::cout << "    translation x: " <<  translation_x << " quaternion w: " << quaternion_w << std::endl;
+         }
+
+         // Rate limit to max 20Hz
+         if (GetCurrentTimestamp() - last_status_time > 50) {
+
+            last_status_time = GetCurrentTimestamp();
+
+            // Create a map to send as a bitset
+            std::map<std::string, bool> bitset;
+            bitset.insert(std::pair<std::string, bool>("PTZ mode", true));
+            bitset.insert(std::pair<std::string, bool>("Walk mode", false));
+            bitset.insert(std::pair<std::string, bool>("Has lease", true));
+            bitset.insert(std::pair<std::string, bool>("Has estop", false));
+            bitset.insert(std::pair<std::string, bool>("FLIR online", true));
+
+            PostBitsetDatapoint("Status", bitset);
+         }
+      }
+      Status status = stream->Finish();
+
+         return;
+   }
+
+   void PostBitsetDatapoint(
+      const std::string &stream,
+      const std::map<std::string, bool> &bitset_map
+   ) {
+      std::cout << "posting bitset datapoint on stream '" << stream << "'" << std::endl;
+
+      Datapoint datapoint;
+
+      // Set the stream name
+      datapoint.set_stream(stream);
+
+      // Set the timestamp to now
+      datapoint.set_timestamp(GetCurrentTimestamp());
+
+      // Create the bitset from the map
+      for (auto const& element : bitset_map) {
+         auto bit = datapoint.mutable_bitset()->add_bits();
+         bit->set_key(element.first);
+         bit->set_value(element.second);
+      }
+
+      // Send the datapoint to the Formant Agent
+      PostDataResponse response;
+      ClientContext context;
+      Status status = stub_->PostData(&context, datapoint, &response);
+
+      if (!status.ok()) {
+         std::cout << "gRPC error: " << status.error_code() << ": " << status.error_message() << std::endl;
+      } else {
+         std::cout << "complete" << std::endl;
+      }
+   }
+
+private:
+   std::unique_ptr<Agent::Stub> stub_;
+
+   long int last_status_time{0};
+
+   long int GetCurrentTimestamp() {
+      struct timeval tp;
+      gettimeofday(&tp, NULL);
+
+      return tp.tv_sec * 1000 + tp.tv_usec / 1000;
+   }
+};
+
+int main(int argc, char **argv)
+{
+   // Create the Formant Agent client object
+   FormantAgentClient client(grpc::CreateChannel("localhost:5501", grpc::InsecureChannelCredentials()));
+
+   // Start listening for commands
+   client.Teleop();
+
+   return 0;
+}

examples/cpp/teleop_heartbeat_callback.cpp

@@ -0,0 +1,67 @@
+#include <iostream>
+#include <limits.h>
+#include <memory>
+#include <string>
+#include <sys/time.h>
+#include <unistd.h>
+#include <signal.h>
+#include <thread>
+
+#include <grpcpp/grpcpp.h>
+
+#include "agent/v1/agent.grpc.pb.h"
+
+using grpc::Channel;
+using grpc::ClientContext;
+using grpc::ClientAsyncResponseReader;
+using grpc::CompletionQueue;
+using grpc::Status;
+
+using v1::agent::Agent;
+using v1::agent::GetTeleopHeartbeatStreamRequest;
+using v1::agent::GetTeleopHeartbeatStreamResponse;
+
+class FormantAgentClient
+{
+public:
+   FormantAgentClient(std::shared_ptr<Channel> channel) : stub_(Agent::NewStub(channel)) {}
+
+   void Listen()
+   {
+      std::cout << "beginning to listen for data..." << std::endl;
+
+      ClientContext context;
+      GetTeleopHeartbeatStreamRequest request;
+      auto stream = stub_->GetTeleopHeartbeatStream(&context, request);
+
+      // Handle an infinite blocking list of commands
+      GetTeleopHeartbeatStreamResponse message;
+      while (stream->Read(&message)) {
+
+         std::cout << "Received heartbeat,  disconnect state: " << message.is_disconnect() << std::endl;
+
+      }
+
+      std::cout << "ending listen for data..." << std::endl;
+   }
+
+private:
+
+   std::unique_ptr<Agent::Stub> stub_;
+
+};
+
+int main(int argc, char **argv)
+{
+   // Create the Formant Agent client object
+   FormantAgentClient client(grpc::CreateChannel("localhose:5501", grpc::InsecureChannelCredentials()));
+
+   std::cout << "Press control-c to quit" << std::endl << std::endl;
+
+   // Start listening for commands
+   std::thread thread_ = std::thread(&FormantAgentClient::Listen, &client);
+
+   thread_.join();  // blocks forever
+
+   return 0;
+}