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

kobayashina 2023-09-28

fix cmd_vel decided by LStick

@c59346a9ca88ac4277dcfccfd07455d2441e8d90

bcafaba

c59346a

README.md

--- README.md

+++ README.md


 | L2              | `float` | `pressedL2Analog();`  | `-1.0`  : Pressed,  `1.0`  : Not pressed                          |
 
 
+## Convert from Left Joy Stick to cmd_vel
+`teleop_twist_joy_node` has cmd_vel publisher.
+Velocity is decided as below.
+![Image](./media/joy2vel.png)
+
 
 ## Acknowledgements
 We acknowledge attribute and gratitude to the following resources in creating this package.

c59346a

media/joy2vel.png (Binary) (added)

+++ media/joy2vel.png

Binary file is not shown

bcafaba

c59346a

p9n_node/src/teleop_twist_joy_node.cpp

--- p9n_node/src/teleop_twist_joy_node.cpp

+++ p9n_node/src/teleop_twist_joy_node.cpp


   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->declare_parameter<double>("linear_speed", 0.9);
   this->angular_max_speed_ =
     this->declare_parameter<double>("angular_speed", 0.6);
 

   this->timer_watchdog_->reset();
   this->p9n_if_->setJoyMsg(joy_msg);
 
+  const double PI = 3.14159;
   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());
 
-    if (twist_msg->linear.x > 0) {
+    if (this->p9n_if_->tiltedStickLY() > sin(PI / 8)) {
       twist_msg->angular.set__z(this->angular_max_speed_ * this->p9n_if_->tiltedStickLX());
-    } else {
+    } else if (this->p9n_if_->tiltedStickLY() < sin(-PI / 8)) {
       twist_msg->angular.set__z(-this->angular_max_speed_ * this->p9n_if_->tiltedStickLX());
+    } else {
+      twist_msg->linear.set__x(0.0);
+      twist_msg->angular.set__z(this->angular_max_speed_ * this->p9n_if_->tiltedStickLX());
     }
     this->twist_pub_->publish(std::move(twist_msg));
 

Add a comment

Open 0
Closed 0

List

...	...	@@ -71,6 +71,11 @@
71	71	\| L2 \| `float` \| `pressedL2Analog();` \| `-1.0` : Pressed, `1.0` : Not pressed \|
72	72
73	73
	74	+## Convert from Left Joy Stick to cmd_vel
	75	+`teleop_twist_joy_node` has cmd_vel publisher.
	76	+Velocity is decided as below.
	77	+![Image](./media/joy2vel.png)
	78	+
74	79
75	80	## Acknowledgements
76	81	We acknowledge attribute and gratitude to the following resources in creating this package.

...	...	@@ -23,7 +23,7 @@
23	23	const std::string hw_name = this->declare_parameter<std::string>(
24	24	"hw_type", p9n_interface::HW_NAME::DUALSENSE);
25	25	this->linear_max_speed_ =
26		- this->declare_parameter<double>("linear_speed", 0.2);
	26	+ this->declare_parameter<double>("linear_speed", 0.9);
27	27	this->angular_max_speed_ =
28	28	this->declare_parameter<double>("angular_speed", 0.6);
29	29
...	...	@@ -64,15 +64,19 @@
64	64	this->timer_watchdog_->reset();
65	65	this->p9n_if_->setJoyMsg(joy_msg);
66	66
	67	+ const double PI = 3.14159;
67	68	static bool stopped = true;
68	69	if (this->p9n_if_->isTiltedStickL()) {
69	70	auto twist_msg = std::make_unique<Twist>();
70	71	twist_msg->linear.set__x(this->linear_max_speed_ * this->p9n_if_->tiltedStickLY());
71	72
72		- if (twist_msg->linear.x > 0) {
	73	+ if (this->p9n_if_->tiltedStickLY() > sin(PI / 8)) {
73	74	twist_msg->angular.set__z(this->angular_max_speed_ * this->p9n_if_->tiltedStickLX());
74		- } else {
	75	+ } else if (this->p9n_if_->tiltedStickLY() < sin(-PI / 8)) {
75	76	twist_msg->angular.set__z(-this->angular_max_speed_ * this->p9n_if_->tiltedStickLX());
	77	+ } else {
	78	+ twist_msg->linear.set__x(0.0);
	79	+ twist_msg->angular.set__z(this->angular_max_speed_ * this->p9n_if_->tiltedStickLX());
76	80	}
77	81	this->twist_pub_->publish(std::move(twist_msg));
78	82

Delete comment