Merge pull request #41 from HarvestX/fix/left_stick_velocity

fix cmd_vel decided by LStick

README.md

@@ -71,6 +71,11 @@ void Example::onJoyCallback(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg) {
 | 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.

p9n_node/include/p9n_node/teleop_twist_joy_node.hpp

@@ -32,7 +32,6 @@ class TeleopTwistJoyNode : public rclcpp::Node
   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_;

p9n_node/src/teleop_twist_joy_node.cpp

@@ -69,10 +69,13 @@ void TeleopTwistJoyNode::onJoy(Joy::ConstSharedPtr joy_msg)
     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(M_PI * 0.125)) {
       twist_msg->angular.set__z(this->angular_max_speed_ * this->p9n_if_->tiltedStickLX());
-    } else {
+    } else if (this->p9n_if_->tiltedStickLY() < sin(-M_PI * 0.125)) {
       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));