malloc_hook exception

Author: jacquelinekay

pendulum_control/include/pendulum_control/pendulum_controller.hpp

@@ -64,7 +64,7 @@ class PendulumController
 
   /// Calculate new command based on new sensor state and PID controller properties.
   // \param[in] msg Received sensor message.
-  void on_sensor_message(const pendulum_msgs::msg::JointState::SharedPtr msg)
+  void on_sensor_message(pendulum_msgs::msg::JointState::ConstSharedPtr msg)
   {
     ++messages_received;
 
@@ -96,10 +96,10 @@ class PendulumController
     message_ready_ = true;
   }
 
-  void on_pendulum_setpoint(const pendulum_msgs::msg::JointCommand::SharedPtr msg)
+  void on_pendulum_setpoint(pendulum_msgs::msg::JointCommand::ConstSharedPtr msg)
   {
     set_command(msg->position);
-    std::cout << "Pendulum set to: " << msg->position << std::endl;
+    printf("Pendulum set to: %f\n", msg->position);
   }
 
   /// Retrieve the command calculated from the last sensor message.

pendulum_control/include/pendulum_control/pendulum_motor.hpp

@@ -77,7 +77,7 @@ class PendulumMotor
 
     // Initialize a separate high-priority thread to run the physics update loop.
     pthread_attr_init(&thread_attr_);
-    struct sched_param thread_param;
+    sched_param thread_param;
     thread_param.sched_priority = 90;
     pthread_attr_setschedparam(&thread_attr_, &thread_param);
     pthread_attr_setschedpolicy(&thread_attr_, SCHED_RR);
@@ -87,7 +87,7 @@ class PendulumMotor
 
   /// Update the position of motor based on the command.
   // \param[in] msg Received command.
-  void on_command_message(const pendulum_msgs::msg::JointCommand::SharedPtr msg)
+  void on_command_message(pendulum_msgs::msg::JointCommand::ConstSharedPtr msg)
   {
     ++messages_received;
     // Assume direct, instantaneous position control
@@ -108,7 +108,7 @@ class PendulumMotor
 
   /// Return the next sensor message calculated by the physics engine.
   // \return The sensor message
-  const pendulum_msgs::msg::JointState::SharedPtr get_next_sensor_message() const
+  pendulum_msgs::msg::JointState::SharedPtr get_next_sensor_message() const
   {
     return sensor_message_;
   }
@@ -221,7 +221,7 @@ class PendulumMotor
   std::chrono::nanoseconds publish_period_;
 
   // Physics should update most frequently, in separate RT thread
-  struct timespec physics_update_timespec_;
+  timespec physics_update_timespec_;
   double dt_;
 
   // Physical qualities of the pendulum

pendulum_control/include/pendulum_control/rtt_executor.hpp

@@ -44,6 +44,7 @@ class RttExecutor : public executor::Executor
   : executor::Executor(ms), running(false)
   {
     rttest_ready = rttest_running();
+    memset(&start_time_, 0, sizeof(timespec));
   }
 
   /// Default destructor
@@ -65,7 +66,7 @@ class RttExecutor : public executor::Executor
 
   /// Retrieve the results measured by rttest
   // \param[in] output A struct containing performance statistics.
-  void get_rtt_results(struct rttest_results & output) const
+  void get_rtt_results(rttest_results & output) const
   {
     output = results;
   }
@@ -78,7 +79,7 @@ class RttExecutor : public executor::Executor
     msg->max_latency = results.max_latency;
     msg->minor_pagefaults = results.minor_pagefaults;
     msg->major_pagefaults = results.major_pagefaults;
-    struct timespec curtime;
+    timespec curtime;
     clock_gettime(CLOCK_MONOTONIC, &curtime);
     msg->stamp.sec = curtime.tv_sec;
     msg->stamp.nanosec = curtime.tv_nsec;
@@ -141,7 +142,7 @@ class RttExecutor : public executor::Executor
   }
 
   // For storing accumulated performance statistics.
-  struct rttest_results results;
+  rttest_results results;
   // True if the executor is spinning.
   bool running;
   // True if rttest has initialized and hasn't been stopped yet.
@@ -151,7 +152,7 @@ class RttExecutor : public executor::Executor
 
 protected:
   // Absolute timestamp at which the first data point was collected in rttest.
-  struct timespec start_time_;
+  timespec start_time_;
 
 private:
   RCLCPP_DISABLE_COPY(RttExecutor);

pendulum_control/scripts/pendulum_launch.bash

@@ -5,21 +5,15 @@ logger_pid=$!
 
 # run pendulum demo indefinitely
 pendulum_demo__rmw_opensplice_cpp -i 0 &
-#pendulum_demo__rmw_opensplice_cpp -i 0
 demo_pid=$!
 
 # Initialize system stress
 stress -c 2 -i 2 --timeout 18000000000 &
 stress_pid=$!
-# Publish new command messages every 3 minutes
-# Run for 5 hours (5*60/3 = 100 iterations)
-# run for 1 hr: 60/3 = 20 iterations
-
 for i in {1..100}; do
   sleep 3m
   pendulum_teleop__rmw_opensplice_cpp $i
 done
 
 # Kill backgrounded processes on exit
 kill -9 $logger_pid $demo_pid $stress_pid
-#kill -9 $logger_pid

pendulum_control/src/pendulum_demo.cpp

@@ -32,10 +32,13 @@
 #include "pendulum_control/pendulum_motor.hpp"
 #include "pendulum_control/rtt_executor.hpp"
 
+
+static bool running = false;
+
 // Initialize a malloc hook so we can show that no mallocs are made during real-time execution
 
 /// Declare a function pointer into which we will store the default malloc.
-static void *(*prev_malloc_hook)(size_t, const void *);
+static void * (* prev_malloc_hook)(size_t, const void *);
 
 // Use pragma to ignore a warning for using __malloc_hook, which is deprecated (but still awesome).
 #pragma GCC diagnostic push
@@ -51,28 +54,20 @@ static void *(*prev_malloc_hook)(size_t, const void *);
 static void * testing_malloc(size_t size, const void * caller)
 {
   (void)caller;
+  /*
   // Maximum depth we are willing to traverse into the stack trace.
   static const int MAX_DEPTH = 2;
   // Instantiate a buffer to store the traced symbols.
   void * backtrace_buffer[MAX_DEPTH];
+  */
+  if (running) {
+    throw std::runtime_error("Called malloc during realtime execution phase!");
+  }
+
   // Set the malloc implementation to the default malloc hook so that we can call it implicitly
   // to initialize a string, otherwise this function will loop infinitely.
   __malloc_hook = prev_malloc_hook;
-  // Backtrace and get the depth of the stack that we traversed.
-  int stack_depth = backtrace(backtrace_buffer, MAX_DEPTH);
-  // Format the symbols as human-readable strings if possible.
-  char ** function_names = backtrace_symbols(backtrace_buffer, stack_depth);
-  // The 0th level of the stack is not very useful; try printing the 1st level.
-  if (stack_depth > 1) {
-    printf("malloc(%u) called from %s [%p]\n",
-      (unsigned)size, function_names[1], backtrace_buffer[1]);
-  } else {
-    // If the first level was unavailable, default to the 0th level.
-    printf("malloc(%u) called from %s [%p]\n",
-      (unsigned)size, function_names[0], backtrace_buffer[0]);
-  }
-  // Release the string that was allocated for printing.
-  free(function_names);
+
   // Execute the requested malloc.
   void * mem = malloc(size);
   // Set the malloc hook back to this function, so that we can intercept future mallocs.
@@ -177,7 +172,7 @@ int main(int argc, char * argv[])
 
   // Create a lambda function to invoke the motor callback when a command is received.
   auto motor_subscribe_callback =
-    [&pendulum_motor](const pendulum_msgs::msg::JointCommand::SharedPtr msg) -> void
+    [&pendulum_motor](pendulum_msgs::msg::JointCommand::ConstSharedPtr msg) -> void
     {
       pendulum_motor->on_command_message(msg);
     };
@@ -190,7 +185,7 @@ int main(int argc, char * argv[])
 
   // Create a lambda function to invoke the controller callback when a command is received.
   auto controller_subscribe_callback =
-    [&pendulum_controller](const pendulum_msgs::msg::JointState::SharedPtr msg) -> void
+    [&pendulum_controller](pendulum_msgs::msg::JointState::ConstSharedPtr msg) -> void
     {
       pendulum_controller->on_sensor_message(msg);
     };
@@ -207,14 +202,14 @@ int main(int argc, char * argv[])
 
   // Create a lambda function to accept user input to command the pendulum
   auto controller_command_callback =
-    [&pendulum_controller](const pendulum_msgs::msg::JointCommand::SharedPtr msg) -> void
+    [&pendulum_controller](pendulum_msgs::msg::JointCommand::ConstSharedPtr msg) -> void
     {
       pendulum_controller->on_pendulum_setpoint(msg);
     };
 
-  auto setpoint_sub = controller_node->create_subscription<pendulum_msgs::msg::JointCommand>
-      ("pendulum_setpoint", qos_profile, controller_command_callback, nullptr, false,
-      setpoint_msg_strategy);
+  auto setpoint_sub = controller_node->create_subscription<pendulum_msgs::msg::JointCommand>(
+    "pendulum_setpoint", qos_profile, controller_command_callback, nullptr, false,
+    setpoint_msg_strategy);
 
   // Initialize the logger publisher.
   auto logger_pub = controller_node->create_publisher<pendulum_msgs::msg::RttestResults>(
@@ -267,8 +262,8 @@ int main(int argc, char * argv[])
   auto controller_publisher_timer = controller_node->create_wall_timer
       (pendulum_controller->get_publish_period(), controller_publish_callback);
   // Add a timer to enable regular publication of results messages.
-  auto logger_publisher_timer = controller_node->create_wall_timer
-      (logger_publisher_period, logger_publish_callback);
+  auto logger_publisher_timer = controller_node->create_wall_timer(
+    logger_publisher_period, logger_publish_callback);
 
   // Set the priority of this thread to the maximum safe value, and set its scheduling policy to a
   // deterministic (real-time safe) algorithm, round robin.
@@ -291,6 +286,7 @@ int main(int argc, char * argv[])
   // End initialization phase
 
   // Execution phase
+  running = true;
 
   // Unlike the default SingleThreadedExecutor::spin function, RttExecutor::spin runs in
   // bounded time (for as many iterations as specified in the rttest parameters).
@@ -301,6 +297,8 @@ int main(int argc, char * argv[])
   // End execution phase
 
   // Teardown phase
+  // deallocation is handled automatically by objects going out of scope
+  running = false;
 
   printf("PendulumMotor received %lu messages\n", pendulum_motor->messages_received);
   printf("PendulumController received %lu messages\n", pendulum_controller->messages_received);

pendulum_control/src/pendulum_logger.cpp

@@ -30,12 +30,14 @@ int main(int argc, char * argv[])
 
   auto logger_node = rclcpp::node::Node::make_shared("pendulum_logger");
   std::string filename = "pendulum_logger_results.csv";
+  std::ofstream fstream;
   {
-    std::ofstream fstream;
     fstream.open(filename, std::ios_base::out);
     fstream << "iteration timestamp latency minor_pagefaults minor_pagefaults" << std::endl;
     fstream.close();
   }
+
+  fstream.open(filename, std::ios_base::app);
   size_t i = 0;
   auto logging_callback =
     [&filename, &i](const pendulum_msgs::msg::RttestResults::SharedPtr msg) {
@@ -52,15 +54,13 @@ int main(int argc, char * argv[])
       printf("Major pagefaults during execution: %lu\n\n", msg->major_pagefaults);
 
       std::ofstream fstream;
-      fstream.open(filename, std::ios_base::app);
       struct timespec timestamp;
       timestamp.tv_sec = msg->stamp.sec;
       timestamp.tv_nsec = msg->stamp.nanosec;
       fstream << i << " " << timespec_to_long(&timestamp) <<
         " " << msg->cur_latency << " " <<
         msg->minor_pagefaults << " " <<
         msg->major_pagefaults << std::endl;
-      fstream.close();
       ++i;
     };
 
@@ -73,14 +73,12 @@ int main(int argc, char * argv[])
   qos_profile.reliability = RMW_QOS_POLICY_BEST_EFFORT;
   // The "KEEP_LAST" history setting tells DDS to store a fixed-size buffer of values before they
   // are sent, to aid with recovery in the event of dropped messages.
-  // "depth" specifies the size of this buffer.
-  // In this example, we are optimizing for performance and limited resource usage (preventing page
-  // faults), instead of reliability. Thus, we set the size of the history buffer to 1.
   qos_profile.history = RMW_QOS_POLICY_KEEP_LAST_HISTORY;
-  qos_profile.depth = 1;
+  qos_profile.depth = 100;
 
   auto subscription = logger_node->create_subscription<pendulum_msgs::msg::RttestResults>(
     "pendulum_statistics", qos_profile, logging_callback);
 
   rclcpp::spin(logger_node);
+  fstream.close();
 }

pendulum_msgs/msg/RttestResults.msg

@@ -1,7 +1,7 @@
 builtin_interfaces/Time stamp
 
-pendulum_msgs/JointCommand command
-pendulum_msgs/JointState state
+JointCommand command
+JointState state
 
 uint64 cur_latency
 float64 mean_latency