Commit @c1457a97955f00de7463938cc450bc4f686e0e50 - yjyoon/ros2_플레이스테이션

WATANABE Aoi 2022-12-14

Merge pull request #21 from HarvestX/fix/twist-node

add watchdog and fix format

@c1457a97955f00de7463938cc450bc4f686e0e50

8c31241

.vscode/cmake-format (deleted)

--- .vscode/cmake-format

...	...	@@ -1,70 +0,0 @@
	1	-# -----------------------------
	2	-# Options effecting formatting.
	3	-# -----------------------------
	4	-with section("format"):
	5	- line_width = 80
	6	- tab_size = 2
	7	- use_tabchars = False
	8	- fractional_tab_policy = 'round-up'
	9	- separate_ctrl_name_with_space = False
	10	- separate_fn_name_with_space = False
	11	- dangle_parens = True
	12	- dangle_align = 'prefix'
	13	- enable_sort = True
	14	-
	15	-# ----------------------------
	16	-# Options affecting the linter
	17	-# ----------------------------
	18	-with section("lint"):
	19	-
	20	- # a list of lint codes to disable
	21	- disabled_codes = ['C0113']
	22	-
	23	- # regular expression pattern describing valid function names
	24	- function_pattern = '[0-9a-z_]+'
	25	-
	26	- # regular expression pattern describing valid macro names
	27	- macro_pattern = '[0-9A-Z_]+'
	28	-
	29	- # regular expression pattern describing valid names for variables with global
	30	- # (cache) scope
	31	- global_var_pattern = '[A-Z][0-9A-Z_]+'
	32	-
	33	- # regular expression pattern describing valid names for variables with global
	34	- # scope (but internal semantic)
	35	- internal_var_pattern = '_[A-Z][0-9A-Z_]+'
	36	-
	37	- # regular expression pattern describing valid names for variables with local
	38	- # scope
	39	- local_var_pattern = '[a-z][a-z0-9_]+'
	40	-
	41	- # regular expression pattern describing valid names for privatedirectory
	42	- # variables
	43	- private_var_pattern = '_[0-9a-z_]+'
	44	-
	45	- # regular expression pattern describing valid names for public directory
	46	- # variables
	47	- public_var_pattern = '[A-Z][0-9A-Z_]+'
	48	-
	49	- # regular expression pattern describing valid names for function/macro
	50	- # arguments and loop variables.
	51	- argument_var_pattern = '[a-z][a-z0-9_]+'
	52	-
	53	- # regular expression pattern describing valid names for keywords used in
	54	- # functions or macros
	55	- keyword_pattern = '[A-Z][0-9A-Z_]+'
	56	-
	57	- # In the heuristic for C0201, how many conditionals to match within a loop in
	58	- # before considering the loop a parser.
	59	- max_conditionals_custom_parser = 2
	60	-
	61	- # Require at least this many newlines between statements
	62	- min_statement_spacing = 1
	63	-
	64	- # Require no more than this many newlines between statements
	65	- max_statement_spacing = 2
	66	- max_returns = 6
	67	- max_branches = 12
	68	- max_arguments = 5
	69	- max_localvars = 15
	70	- max_statements = 50

8c31241

c1457a9

.vscode/cspell.json

--- .vscode/cspell.json

+++ .vscode/cspell.json


     "rclcpp",
     "roboteq",
     "rosdistro",
+    "rosidl",
     "rviz",
     "teleop",
     "urdf",

8c31241

c1457a9

.vscode/settings.json

--- .vscode/settings.json

+++ .vscode/settings.json


   "uncrustify.configPath.linux": ".vscode/uncrustify.cfg",
   "xml.format.splitAttributes": true,
   "editor.formatOnSave": true,
-  "cmakeFormat.args": [
-    "--config-file",
-    ".vscode/cmake-format"
-  ],
   "python.linting.enabled": true,
   "python.linting.flake8Enabled": true,
-  "files.associations": {
-    "*.ui": "xml",
-    "qapplication": "cpp",
-    "chrono": "cpp",
-    "cctype": "cpp",
-    "clocale": "cpp",
-    "cmath": "cpp",
-    "csignal": "cpp",
-    "cstddef": "cpp",
-    "cstdio": "cpp",
-    "cstdlib": "cpp",
-    "cstring": "cpp",
-    "ctime": "cpp",
-    "cwchar": "cpp",
-    "array": "cpp",
-    "atomic": "cpp",
-    "*.tcc": "cpp",
-    "condition_variable": "cpp",
-    "cstdint": "cpp",
-    "list": "cpp",
-    "unordered_map": "cpp",
-    "unordered_set": "cpp",
-    "vector": "cpp",
-    "exception": "cpp",
-    "algorithm": "cpp",
-    "functional": "cpp",
-    "iterator": "cpp",
-    "map": "cpp",
-    "memory": "cpp",
-    "memory_resource": "cpp",
-    "optional": "cpp",
-    "random": "cpp",
-    "ratio": "cpp",
-    "set": "cpp",
-    "string": "cpp",
-    "string_view": "cpp",
-    "system_error": "cpp",
-    "tuple": "cpp",
-    "type_traits": "cpp",
-    "utility": "cpp",
-    "future": "cpp",
-    "initializer_list": "cpp",
-    "iosfwd": "cpp",
-    "iostream": "cpp",
-    "istream": "cpp",
-    "limits": "cpp",
-    "mutex": "cpp",
-    "new": "cpp",
-    "ostream": "cpp",
-    "shared_mutex": "cpp",
-    "sstream": "cpp",
-    "stdexcept": "cpp",
-    "streambuf": "cpp",
-    "thread": "cpp",
-    "cinttypes": "cpp",
-    "typeindex": "cpp",
-    "typeinfo": "cpp",
-    "variant": "cpp",
-    "bit": "cpp",
-    "cstdarg": "cpp",
-    "cwctype": "cpp",
-    "bitset": "cpp",
-    "codecvt": "cpp",
-    "complex": "cpp",
-    "deque": "cpp",
-    "forward_list": "cpp",
-    "numeric": "cpp",
-    "fstream": "cpp",
-    "iomanip": "cpp",
-    "*.ipp": "cpp",
-    "core": "cpp",
-    "strstream": "cpp",
-    "cfenv": "cpp",
-    "valarray": "cpp",
-    "any": "cpp",
-    "regex": "cpp"
-  }
 }

8c31241

c1457a9

p9n_node/CMakeLists.txt

--- p9n_node/CMakeLists.txt

+++ p9n_node/CMakeLists.txt


 
 # Display Node ======================================================
 set(TARGET teleop_twist_joy_node)
-ament_auto_add_library(
-  ${TARGET}
-    SHARED
-      src/${TARGET}.cpp)
+set(MY_LIB_NAME ${PROJECT_NAME}_${TARGET})
+ament_auto_add_library(${MY_LIB_NAME} SHARED src/${TARGET}.cpp)
 rclcpp_components_register_node(
-  ${TARGET}
-    PLUGIN "p9n_node::TeleopTwistJoyNode"
-    EXECUTABLE ${TARGET}_exec)
-# End Display Node ==================================================
+  ${MY_LIB_NAME}
+  PLUGIN "${PROJECT_NAME}::TeleopTwistJoyNode"
+  EXECUTABLE ${TARGET}_exec)
 
+# Testing ===========================================================
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   set(ament_cmake_copyright_FOUND TRUE)

8c31241

c1457a9

p9n_node/include/p9n_node/teleop_twist_joy_node.hpp

--- p9n_node/include/p9n_node/teleop_twist_joy_node.hpp

+++ p9n_node/include/p9n_node/teleop_twist_joy_node.hpp


 {
 class TeleopTwistJoyNode : public rclcpp::Node
 {
+public:
+  using Twist = geometry_msgs::msg::Twist;
+  using Joy = sensor_msgs::msg::Joy;
+
 private:
   float speed_factor_ = 1.0;
+  double linear_max_speed_, angular_max_speed_;
 
   p9n_interface::HW_TYPE hw_type_;
   std::unique_ptr<p9n_interface::PlayStationInterface> p9n_if_;
 
-  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
-  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
+  rclcpp::Subscription<Joy>::SharedPtr joy_sub_;
+  rclcpp::Publisher<Twist>::SharedPtr twist_pub_;
+
+  rclcpp::TimerBase::SharedPtr timer_watchdog_;
 
 public:
-  TeleopTwistJoyNode(const rclcpp::NodeOptions & options);
-  void onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg);
+  TeleopTwistJoyNode() = delete;
+  explicit TeleopTwistJoyNode(const rclcpp::NodeOptions & options);
+  void onJoy(Joy::ConstSharedPtr joy_msg);
+  void onWatchdog();
 };
-}
+}  // namespace p9n_node
 
 #include "rclcpp_components/register_node_macro.hpp"
 RCLCPP_COMPONENTS_REGISTER_NODE(p9n_node::TeleopTwistJoyNode)

8c31241

c1457a9

p9n_node/src/teleop_twist_joy_node.cpp

--- p9n_node/src/teleop_twist_joy_node.cpp

+++ p9n_node/src/teleop_twist_joy_node.cpp


 
 namespace p9n_node
 {
-
 TeleopTwistJoyNode::TeleopTwistJoyNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("teleop_twist_joy_node", options)
 {
-
   const std::string hw_name = this->declare_parameter<std::string>(
     "hw_type", p9n_interface::HW_NAME::DUALSENSE);
+  this->linear_max_speed_ =
+    this->declare_parameter<double>("linear_speed", 0.2);
+  this->angular_max_speed_ =
+    this->declare_parameter<double>("angular_speed", 0.6);
 
   try {
     this->hw_type_ = p9n_interface::getHwType(hw_name);
   } catch (std::runtime_error & e) {
+    RCLCPP_ERROR(this->get_logger(), e.what());
     RCLCPP_ERROR(
-      this->get_logger(),
-      e.what());
-    RCLCPP_ERROR(
-      this->get_logger(),
-      "Please select hardware from %s",
+      this->get_logger(), "Please select hardware from %s",
       p9n_interface::getAllHwName().c_str());
-    rclcpp::shutdown();
+    exit(EXIT_FAILURE);
     return;
   }
 
   this->p9n_if_ =
     std::make_unique<p9n_interface::PlayStationInterface>(this->hw_type_);
 
-  this->joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
-    "joy", rclcpp::SensorDataQoS(rclcpp::KeepLast(1)),
-    std::bind(&TeleopTwistJoyNode::onJoy, this, std::placeholders::_1));
+  using namespace std::placeholders;  // NOLINT
+  this->joy_sub_ = this->create_subscription<Joy>(
+    "joy", rclcpp::SensorDataQoS().keep_last(1),
+    std::bind(&TeleopTwistJoyNode::onJoy, this, _1));
 
-  this->twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
-    "cmd_vel", rclcpp::QoS(10));
+  this->twist_pub_ = this->create_publisher<Twist>(
+    "cmd_vel", rclcpp::QoS(10).reliable().durability_volatile());
 
+
+  using namespace std::chrono_literals; // NOLINT
+  this->timer_watchdog_ = this->create_wall_timer(
+    1s, std::bind(&TeleopTwistJoyNode::onWatchdog, this));
 
   if (this->joy_sub_->get_publisher_count() == 0) {
-    RCLCPP_WARN(
-      this->get_logger(),
-      "Joy node not launched");
+    RCLCPP_WARN(this->get_logger(), "Joy node not launched");
   }
 }
 
-void TeleopTwistJoyNode::onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg)
+void TeleopTwistJoyNode::onJoy(Joy::ConstSharedPtr joy_msg)
 {
+  this->timer_watchdog_->reset();
   this->p9n_if_->setJoyMsg(joy_msg);
 
-  if (this->p9n_if_->pressedR1()) {
-    speed_factor_ = speed_factor_ * 1.1;
-    RCLCPP_INFO(
-      this->get_logger(),
-      "Max speed: %.3f", speed_factor_);
+  static bool stopped = true;
+  if (this->p9n_if_->isTiltedStickL()) {
+    auto twist_msg = std::make_unique<Twist>();
+    twist_msg->linear.set__x(this->linear_max_speed_ * this->p9n_if_->tiltedStickLY());
+    twist_msg->angular.set__z(this->angular_max_speed_ * this->p9n_if_->tiltedStickLX());
+    this->twist_pub_->publish(std::move(twist_msg));
+
+    stopped = false;
+  } else if (!stopped) {
+    // Stop cart
+    auto twist_msg = std::make_unique<Twist>(rosidl_runtime_cpp::MessageInitialization::ZERO);
+    this->twist_pub_->publish(std::move(twist_msg));
+
+    stopped = true;
   }
-  if (this->p9n_if_->pressedL1()) {
-    speed_factor_ = speed_factor_ * 0.9;
-    RCLCPP_INFO(
-      this->get_logger(),
-      "Max speed: %.3f", speed_factor_);
-  }
+}
 
-  auto twist = geometry_msgs::msg::Twist();
-  if (
-    std::abs(this->p9n_if_->tiltedStickLX()) > 1e-2 ||
-    std::abs(this->p9n_if_->tiltedStickLY()) > 1e-2)
-  {
-    float l_y = this->p9n_if_->tiltedStickLY();
-    float l_x = this->p9n_if_->tiltedStickLX();
+void TeleopTwistJoyNode::onWatchdog()
+{
+  RCLCPP_WARN(this->get_logger(), "Couldn't subscribe joy topic before timeout");
 
-    twist.linear.x = speed_factor_ * l_y;
-    twist.angular.z = M_PI * speed_factor_ * l_x;
+  // Publish zero velocity to stop vehicle
+  auto twist_msg = std::make_unique<Twist>(rosidl_runtime_cpp::MessageInitialization::ZERO);
+  this->twist_pub_->publish(std::move(twist_msg));
 
-    this->twist_pub_->publish(twist);
-
-    // never wait for chattering for this scheme
-    return;
-  } else {
-    twist.linear.x = 0.0;
-    twist.angular.z = 0.0;
-    this->twist_pub_->publish(twist);
-  }
-
-  using namespace std::chrono_literals;
-  rclcpp::sleep_for(200ms);
+  this->timer_watchdog_->cancel();
 }
 }  // namespace p9n_node

Add a comment

Open 0
Closed 0

List

...	...	@@ -12,6 +12,7 @@
12	12	"rclcpp",
13	13	"roboteq",
14	14	"rosdistro",
	15	+ "rosidl",
15	16	"rviz",
16	17	"teleop",
17	18	"urdf",

...	...	@@ -18,87 +18,6 @@
18	18	"uncrustify.configPath.linux": ".vscode/uncrustify.cfg",
19	19	"xml.format.splitAttributes": true,
20	20	"editor.formatOnSave": true,
21		- "cmakeFormat.args": [
22		- "--config-file",
23		- ".vscode/cmake-format"
24		- ],
25	21	"python.linting.enabled": true,
26	22	"python.linting.flake8Enabled": true,
27		- "files.associations": {
28		- "*.ui": "xml",
29		- "qapplication": "cpp",
30		- "chrono": "cpp",
31		- "cctype": "cpp",
32		- "clocale": "cpp",
33		- "cmath": "cpp",
34		- "csignal": "cpp",
35		- "cstddef": "cpp",
36		- "cstdio": "cpp",
37		- "cstdlib": "cpp",
38		- "cstring": "cpp",
39		- "ctime": "cpp",
40		- "cwchar": "cpp",
41		- "array": "cpp",
42		- "atomic": "cpp",
43		- "*.tcc": "cpp",
44		- "condition_variable": "cpp",
45		- "cstdint": "cpp",
46		- "list": "cpp",
47		- "unordered_map": "cpp",
48		- "unordered_set": "cpp",
49		- "vector": "cpp",
50		- "exception": "cpp",
51		- "algorithm": "cpp",
52		- "functional": "cpp",
53		- "iterator": "cpp",
54		- "map": "cpp",
55		- "memory": "cpp",
56		- "memory_resource": "cpp",
57		- "optional": "cpp",
58		- "random": "cpp",
59		- "ratio": "cpp",
60		- "set": "cpp",
61		- "string": "cpp",
62		- "string_view": "cpp",
63		- "system_error": "cpp",
64		- "tuple": "cpp",
65		- "type_traits": "cpp",
66		- "utility": "cpp",
67		- "future": "cpp",
68		- "initializer_list": "cpp",
69		- "iosfwd": "cpp",
70		- "iostream": "cpp",
71		- "istream": "cpp",
72		- "limits": "cpp",
73		- "mutex": "cpp",
74		- "new": "cpp",
75		- "ostream": "cpp",
76		- "shared_mutex": "cpp",
77		- "sstream": "cpp",
78		- "stdexcept": "cpp",
79		- "streambuf": "cpp",
80		- "thread": "cpp",
81		- "cinttypes": "cpp",
82		- "typeindex": "cpp",
83		- "typeinfo": "cpp",
84		- "variant": "cpp",
85		- "bit": "cpp",
86		- "cstdarg": "cpp",
87		- "cwctype": "cpp",
88		- "bitset": "cpp",
89		- "codecvt": "cpp",
90		- "complex": "cpp",
91		- "deque": "cpp",
92		- "forward_list": "cpp",
93		- "numeric": "cpp",
94		- "fstream": "cpp",
95		- "iomanip": "cpp",
96		- "*.ipp": "cpp",
97		- "core": "cpp",
98		- "strstream": "cpp",
99		- "cfenv": "cpp",
100		- "valarray": "cpp",
101		- "any": "cpp",
102		- "regex": "cpp"
103		- }
104	23	} (파일 끝에 줄바꿈 문자 없음)

...	...	@@ -10,16 +10,14 @@
10	10
11	11	# Display Node ======================================================
12	12	set(TARGET teleop_twist_joy_node)
13		-ament_auto_add_library(
14		- ${TARGET}
15		- SHARED
16		- src/${TARGET}.cpp)
	13	+set(MY_LIB_NAME ${PROJECT_NAME}_${TARGET})
	14	+ament_auto_add_library(${MY_LIB_NAME} SHARED src/${TARGET}.cpp)
17	15	rclcpp_components_register_node(
18		- ${TARGET}
19		- PLUGIN "p9n_node::TeleopTwistJoyNode"
20		- EXECUTABLE ${TARGET}_exec)
21		-# End Display Node ==================================================
	16	+ ${MY_LIB_NAME}
	17	+ PLUGIN "${PROJECT_NAME}::TeleopTwistJoyNode"
	18	+ EXECUTABLE ${TARGET}_exec)
22	19
	20	+# Testing ===========================================================
23	21	if(BUILD_TESTING)
24	22	find_package(ament_lint_auto REQUIRED)
25	23	set(ament_cmake_copyright_FOUND TRUE)

...	...	@@ -27,20 +27,29 @@
27	27	{
28	28	class TeleopTwistJoyNode : public rclcpp::Node
29	29	{
	30	+public:
	31	+ using Twist = geometry_msgs::msg::Twist;
	32	+ using Joy = sensor_msgs::msg::Joy;
	33	+
30	34	private:
31	35	float speed_factor_ = 1.0;
	36	+ double linear_max_speed_, angular_max_speed_;
32	37
33	38	p9n_interface::HW_TYPE hw_type_;
34	39	std::unique_ptr<p9n_interface::PlayStationInterface> p9n_if_;
35	40
36		- rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
37		- rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
	41	+ rclcpp::Subscription<Joy>::SharedPtr joy_sub_;
	42	+ rclcpp::Publisher<Twist>::SharedPtr twist_pub_;
	43	+
	44	+ rclcpp::TimerBase::SharedPtr timer_watchdog_;
38	45
39	46	public:
40		- TeleopTwistJoyNode(const rclcpp::NodeOptions & options);
41		- void onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg);
	47	+ TeleopTwistJoyNode() = delete;
	48	+ explicit TeleopTwistJoyNode(const rclcpp::NodeOptions & options);
	49	+ void onJoy(Joy::ConstSharedPtr joy_msg);
	50	+ void onWatchdog();
42	51	};
43		-}
	52	+} // namespace p9n_node
44	53
45	54	#include "rclcpp_components/register_node_macro.hpp"
46	55	RCLCPP_COMPONENTS_REGISTER_NODE(p9n_node::TeleopTwistJoyNode)

...	...	@@ -17,85 +17,78 @@
17	17
18	18	namespace p9n_node
19	19	{
20		-
21	20	TeleopTwistJoyNode::TeleopTwistJoyNode(const rclcpp::NodeOptions & options)
22	21	: rclcpp::Node("teleop_twist_joy_node", options)
23	22	{
24		-
25	23	const std::string hw_name = this->declare_parameter<std::string>(
26	24	"hw_type", p9n_interface::HW_NAME::DUALSENSE);
	25	+ this->linear_max_speed_ =
	26	+ this->declare_parameter<double>("linear_speed", 0.2);
	27	+ this->angular_max_speed_ =
	28	+ this->declare_parameter<double>("angular_speed", 0.6);
27	29
28	30	try {
29	31	this->hw_type_ = p9n_interface::getHwType(hw_name);
30	32	} catch (std::runtime_error & e) {
	33	+ RCLCPP_ERROR(this->get_logger(), e.what());
31	34	RCLCPP_ERROR(
32		- this->get_logger(),
33		- e.what());
34		- RCLCPP_ERROR(
35		- this->get_logger(),
36		- "Please select hardware from %s",
	35	+ this->get_logger(), "Please select hardware from %s",
37	36	p9n_interface::getAllHwName().c_str());
38		- rclcpp::shutdown();
	37	+ exit(EXIT_FAILURE);
39	38	return;
40	39	}
41	40
42	41	this->p9n_if_ =
43	42	std::make_unique<p9n_interface::PlayStationInterface>(this->hw_type_);
44	43
45		- this->joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
46		- "joy", rclcpp::SensorDataQoS(rclcpp::KeepLast(1)),
47		- std::bind(&TeleopTwistJoyNode::onJoy, this, std::placeholders::_1));
	44	+ using namespace std::placeholders; // NOLINT
	45	+ this->joy_sub_ = this->create_subscription<Joy>(
	46	+ "joy", rclcpp::SensorDataQoS().keep_last(1),
	47	+ std::bind(&TeleopTwistJoyNode::onJoy, this, _1));
48	48
49		- this->twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
50		- "cmd_vel", rclcpp::QoS(10));
	49	+ this->twist_pub_ = this->create_publisher<Twist>(
	50	+ "cmd_vel", rclcpp::QoS(10).reliable().durability_volatile());
51	51
	52	+
	53	+ using namespace std::chrono_literals; // NOLINT
	54	+ this->timer_watchdog_ = this->create_wall_timer(
	55	+ 1s, std::bind(&TeleopTwistJoyNode::onWatchdog, this));
52	56
53	57	if (this->joy_sub_->get_publisher_count() == 0) {
54		- RCLCPP_WARN(
55		- this->get_logger(),
56		- "Joy node not launched");
	58	+ RCLCPP_WARN(this->get_logger(), "Joy node not launched");
57	59	}
58	60	}
59	61
60		-void TeleopTwistJoyNode::onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg)
	62	+void TeleopTwistJoyNode::onJoy(Joy::ConstSharedPtr joy_msg)
61	63	{
	64	+ this->timer_watchdog_->reset();
62	65	this->p9n_if_->setJoyMsg(joy_msg);
63	66
64		- if (this->p9n_if_->pressedR1()) {
65		- speed_factor_ = speed_factor_ * 1.1;
66		- RCLCPP_INFO(
67		- this->get_logger(),
68		- "Max speed: %.3f", speed_factor_);
	67	+ static bool stopped = true;
	68	+ if (this->p9n_if_->isTiltedStickL()) {
	69	+ auto twist_msg = std::make_unique<Twist>();
	70	+ twist_msg->linear.set__x(this->linear_max_speed_ * this->p9n_if_->tiltedStickLY());
	71	+ twist_msg->angular.set__z(this->angular_max_speed_ * this->p9n_if_->tiltedStickLX());
	72	+ this->twist_pub_->publish(std::move(twist_msg));
	73	+
	74	+ stopped = false;
	75	+ } else if (!stopped) {
	76	+ // Stop cart
	77	+ auto twist_msg = std::make_unique<Twist>(rosidl_runtime_cpp::MessageInitialization::ZERO);
	78	+ this->twist_pub_->publish(std::move(twist_msg));
	79	+
	80	+ stopped = true;
69	81	}
70		- if (this->p9n_if_->pressedL1()) {
71		- speed_factor_ = speed_factor_ * 0.9;
72		- RCLCPP_INFO(
73		- this->get_logger(),
74		- "Max speed: %.3f", speed_factor_);
75		- }
	82	+}
76	83
77		- auto twist = geometry_msgs::msg::Twist();
78		- if (
79		- std::abs(this->p9n_if_->tiltedStickLX()) > 1e-2 \|\|
80		- std::abs(this->p9n_if_->tiltedStickLY()) > 1e-2)
81		- {
82		- float l_y = this->p9n_if_->tiltedStickLY();
83		- float l_x = this->p9n_if_->tiltedStickLX();
	84	+void TeleopTwistJoyNode::onWatchdog()
	85	+{
	86	+ RCLCPP_WARN(this->get_logger(), "Couldn't subscribe joy topic before timeout");
84	87
85		- twist.linear.x = speed_factor_ * l_y;
86		- twist.angular.z = M_PI * speed_factor_ * l_x;
	88	+ // Publish zero velocity to stop vehicle
	89	+ auto twist_msg = std::make_unique<Twist>(rosidl_runtime_cpp::MessageInitialization::ZERO);
	90	+ this->twist_pub_->publish(std::move(twist_msg));
87	91
88		- this->twist_pub_->publish(twist);
89		-
90		- // never wait for chattering for this scheme
91		- return;
92		- } else {
93		- twist.linear.x = 0.0;
94		- twist.angular.z = 0.0;
95		- this->twist_pub_->publish(twist);
96		- }
97		-
98		- using namespace std::chrono_literals;
99		- rclcpp::sleep_for(200ms);
	92	+ this->timer_watchdog_->cancel();
100	93	}
101	94	} // namespace p9n_node

Delete comment