Simplified CMakeLists.txt, added velocity timeout back, added timer for update method.

Author: grayson-arendt

interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt

@@ -23,7 +23,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(trossen_slate REQUIRED)
 
-include_directories(include ${trossen_slate_INCLUDE_DIRS})
+include_directories(include)
 
 set(ROS_DEPENDENCIES
   geometry_msgs
@@ -38,46 +38,16 @@ set(ROS_DEPENDENCIES
   trossen_slate
 )
 
-if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64")
-  set(ARCH "x86_64")
-elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64")
-  set(ARCH "aarch64")
-else()
-  message(FATAL_ERROR "Unknown System Architecture: ${CMAKE_SYSTEM_PROCESSOR}")
-endif()
-
-link_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib/${ARCH})
-
-add_library(slate_base
-  src/slate_base.cpp
-)
-
-ament_target_dependencies(slate_base ${ROS_DEPENDENCIES})
-
-target_link_libraries(slate_base
-${trossen_slate_LIBRARIES}
-)
-
 add_executable(slate_base_node
   src/slate_base_node.cpp
+  src/slate_base.cpp
 )
 
-target_link_libraries(slate_base_node
-  slate_base
-)
-
-install(
-  TARGETS slate_base
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-)
+ament_target_dependencies(slate_base_node ${ROS_DEPENDENCIES})
 
 install(
-  TARGETS
-    slate_base_node
-  RUNTIME DESTINATION
-    lib/${PROJECT_NAME}
+  TARGETS slate_base_node
+  DESTINATION lib/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)

interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.hpp

@@ -76,7 +76,7 @@ class SlateBase
   void update();
 
 private:
-  // Update counter used to only update some values less frequently
+  // Update counter used to update the battery status less frequently
   int cnt_;
 
   // Array containing x and y translation in meters and rotation in radians
@@ -94,7 +94,7 @@ class SlateBase
   // Base command bytes containing data about charging and motor torque enabling
   uint32_t sys_cmd_ = 0;
 
-  // Stored data of the SLATE base
+  // Stored data of the base
   base_driver::ChassisData data_;
 
   // Name of odom frame
@@ -127,9 +127,18 @@ class SlateBase
   // Set light state service server
   rclcpp::Service<SetLightState>::SharedPtr srv_set_light_state_;
 
+  // Timer used to update the base state
+  rclcpp::TimerBase::SharedPtr timer_;
+
   // If publish_tf_ is true, this is the broadcaster used to publish the odom->base_link TF
   tf2_ros::TransformBroadcaster tf_broadcaster_odom_;
 
+  // Timeout for base velocity
+  rclcpp::Duration cmd_vel_timeout_;
+
+  // Time last velocity command was received
+  rclcpp::Time cmd_vel_time_last_update_;
+
   /**
    * @brief Process incoming Twist command message
    * @param msg Incoming Twist command message
@@ -192,6 +201,6 @@ class SlateBase
   float wrap_angle(float angle);
 };
 
-} // namespace slate_base
+}  // namespace slate_base
 
-#endif // INTERBOTIX_SLATE_DRIVER__SLATE_BASE_HPP_
+#endif  // INTERBOTIX_SLATE_DRIVER__SLATE_BASE_HPP_

interbotix_ros_slate/interbotix_slate_driver/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>interbotix_slate_driver</name>
-  <version>1.0.0</version>
+  <version>0.0.0</version>
   <description>The interbotix_slate_driver package</description>
 
   <maintainer email="trsupport@trossenrobotics.com">Trossen Robotics</maintainer>

interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp

@@ -35,8 +35,10 @@ namespace slate_base
 {
 
 SlateBase::SlateBase(const rclcpp::NodeOptions & options)
-: trossen_slate::TrossenSlate(), rclcpp::Node("slate_base", options), sys_cmd_{0},
-  publish_tf_(false), current_time_(get_clock()->now()), tf_broadcaster_odom_(this)
+: trossen_slate::TrossenSlate(), rclcpp::Node("slate_base", options), cnt_(0), pose_{0},
+  publish_tf_(false), is_first_odom_(0), current_time_(get_clock()->now()), tf_broadcaster_odom_(
+    this), cmd_vel_time_last_update_(get_clock()->now()),
+  cmd_vel_timeout_(rclcpp::Duration(std::chrono::milliseconds(CMD_TIME_OUT)))
 {
   using std::placeholders::_1, std::placeholders::_2, std::placeholders::_3;
 
@@ -72,6 +74,8 @@ SlateBase::SlateBase(const rclcpp::NodeOptions & options)
     "set_light_state",
     std::bind(&SlateBase::set_light_state_callback, this, _1, _2, _3));
 
+  timer_ = create_wall_timer(std::chrono::milliseconds(50), std::bind(&SlateBase::update, this));
+
   std::string result;
   if (!init_base(result)) {
     RCLCPP_FATAL(get_logger(), result.c_str());
@@ -84,9 +88,14 @@ SlateBase::SlateBase(const rclcpp::NodeOptions & options)
 
 void SlateBase::update()
 {
-  rclcpp::spin_some(get_node_base_interface());
   current_time_ = get_clock()->now();
 
+  // Time out velocity commands
+  if (current_time_ - cmd_vel_time_last_update_ > cmd_vel_timeout_) {
+    data_.cmd_vel_x = 0.0f;
+    data_.cmd_vel_z = 0.0f;
+  }
+
   if (!base_driver::updateChassisInfo(&data_)) {
     return;
   }
@@ -97,9 +106,12 @@ void SlateBase::update()
   if (cnt_ % 10 == 0) {
     battery_state.header.stamp = current_time_;
     battery_state.voltage = data_.voltage;
+    battery_state.temperature = std::numeric_limits<double>::quiet_NaN();
     battery_state.current = data_.current;
+    battery_state.charge = std::numeric_limits<double>::quiet_NaN();
+    battery_state.capacity = std::numeric_limits<double>::quiet_NaN();
+    battery_state.design_capacity = std::numeric_limits<double>::quiet_NaN();
     battery_state.percentage = data_.charge;
-    battery_state.power_supply_status = data_.system_state;
     pub_battery_state_->publish(battery_state);
   }
 
@@ -157,6 +169,7 @@ void SlateBase::cmd_vel_callback(const Twist::SharedPtr msg)
 {
   data_.cmd_vel_x = msg->linear.x;
   data_.cmd_vel_z = msg->linear.z;
+  cmd_vel_time_last_update_ = get_clock()->now();
 }
 
 bool SlateBase::set_text_callback(
@@ -220,4 +233,4 @@ float SlateBase::wrap_angle(float angle)
   return angle;
 }
 
-} // namespace slate_base
+}  // namespace slate_base

interbotix_ros_slate/interbotix_slate_driver/src/slate_base_node.cpp

@@ -34,11 +34,7 @@ int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
   auto node = std::make_shared<slate_base::SlateBase>();
-
-  auto r = rclcpp::Rate(20);
-
-  while (rclcpp::ok()) {
-    node->update();
-    r.sleep();
-  }
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
 }