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

WATANABE Aoi 2022-07-12

Merge pull request #12 from HarvestX/features/speed-factor

add speed factor

@6ba3d809820f1aab4c0bcb2cadd36e72e579262d

1a9ef34

6ba3d80

.vscode/c_cpp_properties.json

--- .vscode/c_cpp_properties.json

+++ .vscode/c_cpp_properties.json


             "includePath": [
                 "/opt/ros/galactic/include/**",
                 "/usr/include/**",
-                "{workspaceFolder}/p9n_interface/include/**",
-                "{workspaceFolder}/p9n_example/include/**",
-                "{workspaceFolder}/p9n_test/include/**",
+                "${workspaceFolder}/p9n_interface/include/**",
+                "${workspaceFolder}/p9n_example/include/**",
+                "${workspaceFolder}/p9n_test/include/**",
                 "${workspaceFolder}/p9n_node/include/**"
             ],
             "name": "ROS",

1a9ef34

6ba3d80

.vscode/cspell.json

--- .vscode/cspell.json

+++ .vscode/cspell.json


     "urdf",
     "bringup",
     "dualsense",
-    "dualshock"
+    "dualshock",
+    "teleop"
   ]
 }

1a9ef34

6ba3d80

.vscode/settings.json

--- .vscode/settings.json

+++ .vscode/settings.json


     "core": "cpp",
     "strstream": "cpp",
     "cfenv": "cpp",
-    "valarray": "cpp"
+    "valarray": "cpp",
+    "any": "cpp",
+    "regex": "cpp"
   }
 }

1a9ef34

6ba3d80

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
 {
 private:
+  float speed_factor_ = 1.0;
+
   p9n_interface::HW_TYPE hw_type_;
   std::unique_ptr<p9n_interface::PlayStationInterface> p9n_if_;
 

1a9ef34

6ba3d80

p9n_node/src/teleop_twist_joy_node.cpp

--- p9n_node/src/teleop_twist_joy_node.cpp

+++ p9n_node/src/teleop_twist_joy_node.cpp


 void TeleopTwistJoyNode::onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg)
 {
   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_);
+  }
+  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_x = this->p9n_if_->tiltedStickLX();
     float l_y = this->p9n_if_->tiltedStickLY();
+    float l_x = this->p9n_if_->tiltedStickLX();
 
-    auto twist = geometry_msgs::msg::Twist();
-    twist.linear.x = l_y;
-    twist.angular.z = l_x;
+    twist.linear.x = speed_factor_ * l_y;
+    twist.angular.z = M_PI * speed_factor_ * l_x;
+
     this->twist_pub_->publish(twist);
+
+    // never wait for chattering for this scheme
+    return;
   } else {
-    using namespace std::chrono_literals;
-    rclcpp::sleep_for(100ms);
+    twist.linear.x = 0.0;
+    twist.angular.z = 0.0;
+    this->twist_pub_->publish(twist);
   }
+
+  using namespace std::chrono_literals;
+  rclcpp::sleep_for(200ms);
 }
-} // namespace p9n_node
+}  // namespace p9n_node

Add a comment

Open 0
Closed 0

List

...	...	@@ -13,6 +13,7 @@
13	13	"urdf",
14	14	"bringup",
15	15	"dualsense",
16		- "dualshock"
	16	+ "dualshock",
	17	+ "teleop"
17	18	]
18	19	} (파일 끝에 줄바꿈 문자 없음)

...	...	@@ -97,6 +97,8 @@
97	97	"core": "cpp",
98	98	"strstream": "cpp",
99	99	"cfenv": "cpp",
100		- "valarray": "cpp"
	100	+ "valarray": "cpp",
	101	+ "any": "cpp",
	102	+ "regex": "cpp"
101	103	}
102	104	} (파일 끝에 줄바꿈 문자 없음)

...	...	@@ -28,6 +28,8 @@
28	28	class TeleopTwistJoyNode : public rclcpp::Node
29	29	{
30	30	private:
	31	+ float speed_factor_ = 1.0;
	32	+
31	33	p9n_interface::HW_TYPE hw_type_;
32	34	std::unique_ptr<p9n_interface::PlayStationInterface> p9n_if_;
33	35

...	...	@@ -60,20 +60,42 @@
60	60	void TeleopTwistJoyNode::onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg)
61	61	{
62	62	this->p9n_if_->setJoyMsg(joy_msg);
	63	+
	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_);
	69	+ }
	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	+ }
	76	+
	77	+ auto twist = geometry_msgs::msg::Twist();
63	78	if (
64	79	std::abs(this->p9n_if_->tiltedStickLX()) > 1e-2 \|\|
65	80	std::abs(this->p9n_if_->tiltedStickLY()) > 1e-2)
66	81	{
67		- float l_x = this->p9n_if_->tiltedStickLX();
68	82	float l_y = this->p9n_if_->tiltedStickLY();
	83	+ float l_x = this->p9n_if_->tiltedStickLX();
69	84
70		- auto twist = geometry_msgs::msg::Twist();
71		- twist.linear.x = l_y;
72		- twist.angular.z = l_x;
	85	+ twist.linear.x = speed_factor_ * l_y;
	86	+ twist.angular.z = M_PI * speed_factor_ * l_x;
	87	+
73	88	this->twist_pub_->publish(twist);
	89	+
	90	+ // never wait for chattering for this scheme
	91	+ return;
74	92	} else {
75		- using namespace std::chrono_literals;
76		- rclcpp::sleep_for(100ms);
	93	+ twist.linear.x = 0.0;
	94	+ twist.angular.z = 0.0;
	95	+ this->twist_pub_->publish(twist);
77	96	}
	97	+
	98	+ using namespace std::chrono_literals;
	99	+ rclcpp::sleep_for(200ms);
78	100	}
79		-} // namespace p9n_node
	101	+} // namespace p9n_node

...	...	@@ -8,9 +8,9 @@
8	8	"includePath": [
9	9	"/opt/ros/galactic/include/**",
10	10	"/usr/include/**",
11		- "{workspaceFolder}/p9n_interface/include/**",
12		- "{workspaceFolder}/p9n_example/include/**",
13		- "{workspaceFolder}/p9n_test/include/**",
	11	+ "${workspaceFolder}/p9n_interface/include/**",
	12	+ "${workspaceFolder}/p9n_example/include/**",
	13	+ "${workspaceFolder}/p9n_test/include/**",
14	14	"${workspaceFolder}/p9n_node/include/**"
15	15	],
16	16	"name": "ROS",

Delete comment