Get halsim_gazebo building again (#1201)

simulation/halsim_gazebo/src/main/native/cpp/GazeboAnalogIn.cpp

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "GazeboAnalogIn.h"
+
+#include <string>
+
+#include <HAL/Power.h>
+
+#include "MockData/AnalogInData.h"
+#include "MockData/HAL_Value.h"
+#include "MockData/NotifyListener.h"
+
+static void init_callback(const char* name, void* param,
+                          const struct HAL_Value* value) {
+  GazeboAnalogIn* ain = static_cast<GazeboAnalogIn*>(param);
+  ain->SetInitialized(value->data.v_boolean);
+  if (ain->IsInitialized()) {
+    ain->Listen();
+  }
+}
+
+GazeboAnalogIn::GazeboAnalogIn(int index, HALSimGazebo* halsim) {
+  m_index = index;
+  m_halsim = halsim;
+  m_sub = NULL;
+  HALSIM_RegisterAnalogInInitializedCallback(index, init_callback, this, true);
+}
+
+void GazeboAnalogIn::Listen() {
+  if (!m_sub)
+    m_sub = m_halsim->node.Subscribe<gazebo::msgs::Float64>(
+        "~/simulator/analog/" + std::to_string(m_index),
+        &GazeboAnalogIn::Callback, this);
+}
+
+void GazeboAnalogIn::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+  /* This value is going to be divided by the 5V rail in the HAL, so
+     we multiply by that value to make the change neutral */
+  int32_t status;
+  HALSIM_SetAnalogInVoltage(m_index,
+                            msg->data() * HAL_GetUserVoltage5V(&status));
+}

simulation/halsim_gazebo/src/main/native/cpp/GazeboDIO.cpp

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "GazeboDIO.h"
+
+#include <string>
+
+#include "MockData/DIOData.h"
+#include "MockData/HAL_Value.h"
+#include "MockData/NotifyListener.h"
+
+static void init_callback(const char* name, void* param,
+                          const struct HAL_Value* value) {
+  GazeboDIO* dio = static_cast<GazeboDIO*>(param);
+  dio->SetInitialized(value->data.v_boolean);
+  if (dio->IsInitialized()) {
+    dio->Listen();
+  }
+}
+
+GazeboDIO::GazeboDIO(int index, HALSimGazebo* halsim) {
+  m_index = index;
+  m_halsim = halsim;
+  m_sub = NULL;
+  HALSIM_RegisterDIOInitializedCallback(index, init_callback, this, true);
+}
+
+void GazeboDIO::Listen() {
+  if (!m_sub)
+    m_sub = m_halsim->node.Subscribe<gazebo::msgs::Bool>(
+        "~/simulator/dio/" + std::to_string(m_index), &GazeboDIO::Callback,
+        this);
+}
+
+void GazeboDIO::Callback(const gazebo::msgs::ConstBoolPtr& msg) {
+  HALSIM_SetDIOValue(m_index, msg->data());
+}

simulation/halsim_gazebo/src/main/native/cpp/GazeboEncoder.cpp

@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "GazeboEncoder.h"
+
+#include <string>
+
+#include "MockData/EncoderData.h"
+#include "MockData/HAL_Value.h"
+#include "MockData/NotifyListener.h"
+
+static void encoder_init_callback(const char* name, void* param,
+                                  const struct HAL_Value* value) {
+  GazeboEncoder* encoder = static_cast<GazeboEncoder*>(param);
+  encoder->SetInitialized(value->data.v_boolean);
+  if (encoder->IsInitialized()) {
+    encoder->Control("start");
+    encoder->Listen();
+  }
+}
+
+static void encoder_reset_callback(const char* name, void* param,
+                                   const struct HAL_Value* value) {
+  GazeboEncoder* encoder = static_cast<GazeboEncoder*>(param);
+  if (encoder->IsInitialized() && value->data.v_boolean)
+    encoder->Control("reset");
+}
+
+static void encoder_reverse_callback(const char* name, void* param,
+                                     const struct HAL_Value* value) {
+  GazeboEncoder* encoder = static_cast<GazeboEncoder*>(param);
+  if (encoder->IsInitialized()) encoder->SetReverse(value->data.v_boolean);
+}
+
+GazeboEncoder::GazeboEncoder(int index, HALSimGazebo* halsim) {
+  m_index = index;
+  m_halsim = halsim;
+  m_reverse = false;
+  m_pub = NULL;
+  m_sub = NULL;
+  HALSIM_RegisterEncoderInitializedCallback(index, encoder_init_callback, this,
+                                            true);
+  HALSIM_RegisterEncoderResetCallback(index, encoder_reset_callback, this,
+                                      true);
+  HALSIM_RegisterEncoderReverseDirectionCallback(
+      index, encoder_reverse_callback, this, true);
+}
+
+void GazeboEncoder::Control(const char* command) {
+  if (!m_pub) {
+    m_pub = m_halsim->node.Advertise<gazebo::msgs::String>(
+        "~/simulator/encoder/dio/" +
+        std::to_string(HALSIM_GetDigitalChannelA(m_index)) + "/control");
+    m_pub->WaitForConnection(gazebo::common::Time(1, 0));
+  }
+  gazebo::msgs::String msg;
+  msg.set_data(command);
+  if (m_pub) m_pub->Publish(msg);
+}
+
+void GazeboEncoder::Listen() {
+  if (!m_sub)
+    m_sub = m_halsim->node.Subscribe<gazebo::msgs::Float64>(
+        "~/simulator/encoder/dio/" +
+            std::to_string(HALSIM_GetDigitalChannelA(m_index)) + "/position",
+        &GazeboEncoder::Callback, this);
+}
+
+void GazeboEncoder::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+  HALSIM_SetEncoderCount(m_index, msg->data() * (m_reverse ? -1 : 1));
+}

simulation/halsim_gazebo/src/main/native/cpp/GazeboNode.cpp

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "GazeboNode.h"
+
+bool GazeboNode::Connect() {
+  bool success = gazebo::client::setup();
+
+  if (success) {
+    main = gazebo::transport::NodePtr(new gazebo::transport::Node());
+    main->Init("frc");
+    gazebo::transport::run();
+  }
+
+  return success;
+}

simulation/halsim_gazebo/src/main/native/cpp/GazeboPCM.cpp

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "GazeboPCM.h"
+
+#include <string>
+
+#include "MockData/HAL_Value.h"
+#include "MockData/NotifyListener.h"
+#include "MockData/PCMData.h"
+#include "simulation/gz_msgs/msgs.h"
+
+static void init_callback(const char* name, void* param,
+                          const struct HAL_Value* value) {
+  GazeboPCM* pcm = static_cast<GazeboPCM*>(param);
+  pcm->SetInitialized(value->data.v_boolean);
+}
+
+static void output_callback(const char* name, void* param,
+                            const struct HAL_Value* value) {
+  GazeboPCM* pcm = static_cast<GazeboPCM*>(param);
+  if (pcm->IsInitialized()) pcm->Publish(value->data.v_boolean);
+}
+
+GazeboPCM::GazeboPCM(int index, int channel, HALSimGazebo* halsim) {
+  m_index = index;
+  m_channel = channel;
+  m_halsim = halsim;
+  m_pub = NULL;
+  HALSIM_RegisterPCMSolenoidInitializedCallback(index, channel, init_callback,
+                                                this, true);
+  HALSIM_RegisterPCMSolenoidOutputCallback(index, channel, output_callback,
+                                           this, true);
+}
+
+void GazeboPCM::Publish(bool value) {
+  if (!m_pub) {
+    m_pub = m_halsim->node.Advertise<gazebo::msgs::Bool>(
+        "~/simulator/pneumatic/" + std::to_string(m_index + 1) + "/" +
+        std::to_string(m_channel));
+    m_pub->WaitForConnection(gazebo::common::Time(1, 0));
+  }
+  gazebo::msgs::Bool msg;
+  msg.set_data(value);
+  if (m_pub) m_pub->Publish(msg);
+}
+
+void GazeboPCM_SetPressureSwitch(int index, bool value) {
+  HALSIM_SetPCMPressureSwitch(index, value);
+}

simulation/halsim_gazebo/src/main/native/cpp/GazeboPWM.cpp

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "GazeboPWM.h"
+
+#include <string>
+
+#include "MockData/HAL_Value.h"
+#include "MockData/NotifyListener.h"
+#include "MockData/PWMData.h"
+#include "simulation/gz_msgs/msgs.h"
+
+static void init_callback(const char* name, void* param,
+                          const struct HAL_Value* value) {
+  GazeboPWM* pwm = static_cast<GazeboPWM*>(param);
+  pwm->SetInitialized(value->data.v_boolean);
+}
+
+static void speed_callback(const char* name, void* param,
+                           const struct HAL_Value* value) {
+  GazeboPWM* pwm = static_cast<GazeboPWM*>(param);
+  if (pwm->IsInitialized()) pwm->Publish(value->data.v_double);
+}
+
+GazeboPWM::GazeboPWM(int port, HALSimGazebo* halsim) {
+  m_port = port;
+  m_halsim = halsim;
+  HALSIM_RegisterPWMInitializedCallback(port, init_callback, this, true);
+  HALSIM_RegisterPWMSpeedCallback(port, speed_callback, this, true);
+}
+
+void GazeboPWM::Publish(double value) {
+  if (!m_pub) {
+    m_pub = m_halsim->node.Advertise<gazebo::msgs::Float64>(
+        "~/simulator/pwm/" + std::to_string(m_port));
+    m_pub->WaitForConnection(gazebo::common::Time(1, 0));
+  }
+  gazebo::msgs::Float64 msg;
+  msg.set_data(value);
+  if (m_pub) m_pub->Publish(msg);
+}

simulation/halsim_gazebo/src/main/native/cpp/main.cpp

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <iostream>
+
+#include <HAL/Ports.h>
+
+#include "GazeboAnalogIn.h"
+#include "GazeboDIO.h"
+#include "GazeboEncoder.h"
+#include "GazeboPCM.h"
+#include "GazeboPWM.h"
+#include "HALSimGazebo.h"
+
+/* Currently, robots never terminate, so we keep a single static object
+   to access Gazebo with and it is never properly released or cleaned up. */
+static HALSimGazebo halsim;
+
+extern "C" {
+int HALSIM_InitExtension(void) {
+  std::cout << "Gazebo Simulator Initializing." << std::endl;
+
+  if (!halsim.node.Connect()) {
+    std::cerr << "Error: unable to connect to Gazebo.  Is it running?."
+              << std::endl;
+    return -1;
+  }
+  std::cout << "Gazebo Simulator Connected." << std::endl;
+
+  for (int i = 0; i < HALSimGazebo::kPWMCount; i++)
+    halsim.pwms[i] = new GazeboPWM(i, &halsim);
+
+  for (int i = 0; i < HALSimGazebo::kPCMCount; i++)
+    halsim.pcms[i] = new GazeboPCM(0, i, &halsim);
+  GazeboPCM_SetPressureSwitch(0, true);
+
+  for (int i = 0; i < HALSimGazebo::kEncoderCount; i++)
+    halsim.encoders[i] = new GazeboEncoder(i, &halsim);
+
+  int analog_in_count = HAL_GetNumAnalogInputs();
+  for (int i = 0; i < analog_in_count; i++)
+    halsim.analog_inputs.push_back(new GazeboAnalogIn(i, &halsim));
+
+  int dio_count = HAL_GetNumDigitalChannels();
+  for (int i = 0; i < dio_count; i++)
+    halsim.dios.push_back(new GazeboDIO(i, &halsim));
+
+  return 0;
+}
+}  // extern "C"