Arm teleop fixes

src/Arm/arm_control/config/arm_config.yaml

@@ -15,10 +15,10 @@ max_expected_latency: 0.1
 command_in_type: "unitless"  # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
 scale:
   # Scale parameters are only used if command_in_type=="unitless"
-  linear:  0.8  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
-  rotational:  0.8  # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
+  linear:  0.4  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
+  rotational:  0.4  # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
-  joint: 4.0
+  joint: 0.6
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory

src/Arm/arm_control/include/arm_control/MoveGroupNode.hpp

@@ -19,6 +19,7 @@
 #include "interfaces/srv/go_to_named_pose.hpp"
 #include "interfaces/srv/go_to_pose.hpp"
 #include "interfaces/srv/save_current_pose.hpp"
+#include "std_srvs/srv/trigger.hpp"
 
 namespace arm_control {
 
@@ -74,6 +75,10 @@ class MoveGroupNode : public rclcpp::Node {
       const std::shared_ptr<interfaces::srv::GoToCamCoord::Request> request,
       std::shared_ptr<interfaces::srv::GoToCamCoord::Response> response);
 
+  void
+  stopCallback(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+               std::shared_ptr<std_srvs::srv::Trigger::Response> response);
+
   double planning_time_;
   int planning_attempts_;
   double vel_scaling_;
@@ -119,6 +124,8 @@ class MoveGroupNode : public rclcpp::Node {
   rclcpp::Service<interfaces::srv::GoToCamCoord>::SharedPtr
       go_to_cam_coord_service_;
 
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr stop_service_;
+
   rclcpp::TimerBase::SharedPtr initialization_timer_;
 };
 

src/Arm/arm_control/src/MoveGroupNode.cpp

@@ -66,6 +66,12 @@ MoveGroupNode::MoveGroupNode(const rclcpp::NodeOptions &options)
                 std::placeholders::_1, std::placeholders::_2),
       rmw_qos_profile_services_default, service_callback_group_);
 
+  stop_service_ = create_service<std_srvs::srv::Trigger>(
+      "~/stop",
+      std::bind(&MoveGroupNode::stopCallback, this, std::placeholders::_1,
+                std::placeholders::_2),
+      rmw_qos_profile_services_default, service_callback_group_);
+
   publishStatus(interfaces::msg::MoveGroupStatus::IDLE, "Initializing");
 
   initialization_timer_ =
@@ -481,6 +487,27 @@ void MoveGroupNode::goToCamCoordCallback(
   }
 }
 
+void MoveGroupNode::stopCallback(
+    const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+    std::shared_ptr<std_srvs::srv::Trigger::Response> response) {
+  (void)request;
+
+  std::lock_guard<std::mutex> command_lock(command_mutex_);
+
+  if (!initialized_ || !move_group_) {
+    response->success = false;
+    response->message = "MoveGroupInterface is not initialized";
+    return;
+  }
+
+  move_group_->stop();
+
+  response->success = true;
+  response->message = "Motion stopped";
+
+  publishStatus(interfaces::msg::MoveGroupStatus::IDLE, "Idle");
+}
+
 } // namespace arm_control
 
 RCLCPP_COMPONENTS_REGISTER_NODE(arm_control::MoveGroupNode)

src/HW-Devices/hardware/include/TalonSRXWrapper.hpp

@@ -49,6 +49,11 @@ class TalonSRXWrapper : public BaseWrapper {
   static SensorType sensor_type_from_str(std::string str);
   int get_load_enc() const;
   void update_gravity_ff();
+  double ticks_to_rads(double ticks) const;
+  double rads_to_ticks(double rads) const;
+
+  // used in crossover_mode
+  double raw_position_{0.0};
 
   // Parameters
   int id_;

src/HW-Devices/hardware/src/TalonSRXWrapper.cpp

@@ -94,8 +94,7 @@ TalonSRXWrapper::TalonSRXWrapper(const hardware_interface::ComponentInfo &joint,
   debug_pub_ = debug_node_->create_publisher<ros_phoenix::msg::MotorStatus>(
       joint.name + "/status", rclcpp::SystemDefaultsQoS());
 
-  sensor_offset_ticks_ =
-      static_cast<int>(sensor_offset * sensor_ticks_ / (2.0 * M_PI));
+  sensor_offset_ticks_ = static_cast<int>(rads_to_ticks(sensor_offset));
 }
 
 TalonSRXWrapper::SensorType
@@ -155,6 +154,14 @@ void TalonSRXWrapper::pub_status() const {
   debug_pub_->publish(status_msg);
 }
 
+double TalonSRXWrapper::ticks_to_rads(double ticks) const {
+  return ticks * (2.0 * M_PI) / sensor_ticks_;
+}
+
+double TalonSRXWrapper::rads_to_ticks(double rads) const {
+  return rads * sensor_ticks_ / (2.0 * M_PI);
+}
+
 void TalonSRXWrapper::write() {
   if (!initialized_) {
     if ((debug_node_->now() - start_time_).seconds() > kWaitDurationSec) {
@@ -179,64 +186,51 @@ void TalonSRXWrapper::write() {
         __FUNCTION__, id_);
     return;
   }
-  if (std::abs(command_ - position_) > M_PI &&
+  if (!crossover_mode_ && std::abs(command_ - position_) > M_PI &&
       control_type_ == motors::ControlMode::Position) {
     RCLCPP_WARN_THROTTLE(
         debug_node_->get_logger(), *debug_node_->get_clock(), 1000,
         "%s: Large position error (%.2f) for id %d, which may indicate a "
-        "problem with sensor configuration or an unexpected jump in position",
-        __FUNCTION__, command_ - position_, id_);
+        "problem with sensor configuration or an unexpected jump in position "
+        "(%.2f vs %.2f)",
+        __FUNCTION__, command_ - position_, id_, command_, position_);
     return;
   }
   double output = 0.0;
-  if (crossover_mode_ && std::abs(command_) > M_PI) {
-    RCLCPP_WARN_THROTTLE(
-        debug_node_->get_logger(), *debug_node_->get_clock(), 1000,
-        "%s: Command %.2f is outside of [-pi, pi] in crossover mode, which may "
-        "cause unexpected behavior for id %d",
-        __FUNCTION__, command_, id_);
-  }
   if (control_type_ == motors::ControlMode::Position) {
     if (crossover_mode_) {
-      double normalized = (command_ + M_PI) / (2.0 * M_PI);
-      double ticks = normalized * sensor_ticks_;
-      double ticks_with_offset = ticks + sensor_offset_ticks_;
-      output = fmod(ticks_with_offset, sensor_ticks_);
-      if (output < 0)
-        output += sensor_ticks_;
+      const double position_error =
+          std::remainder(command_ - raw_position_, 2.0 * M_PI);
+      const double target_position = raw_position_ + position_error;
+      output = rads_to_ticks(target_position) + sensor_offset_ticks_;
     } else {
-      output = command_ * sensor_ticks_ / (2.0 * M_PI) + sensor_offset_ticks_;
+      output = rads_to_ticks(command_) + sensor_offset_ticks_;
     }
   } else if (control_type_ == motors::ControlMode::Velocity) {
     // Talons use d / 100ms as vel
-    output = (command_ * sensor_ticks_ / (2.0 * M_PI)) / 10.0;
+    output = rads_to_ticks(command_) / 10.0;
   }
   talon_controller_->Set(control_type_, output,
                          motors::DemandType::DemandType_ArbitraryFeedForward,
                          gravity_ff_.load(std::memory_order_relaxed));
 }
 
 void TalonSRXWrapper::read() {
-  int raw_position =
-      talon_controller_->GetSelectedSensorPosition() - sensor_offset_ticks_;
+  const int raw_ticks = talon_controller_->GetSelectedSensorPosition() -
+                        static_cast<int>(sensor_offset_ticks_);
+  raw_position_ = ticks_to_rads(raw_ticks);
   if (crossover_mode_) {
-    raw_position %= sensor_ticks_;
-    if (raw_position < 0) {
-      raw_position += sensor_ticks_;
-    }
-    double normalized = static_cast<double>(raw_position) / sensor_ticks_;
-    position_ = normalized * 2.0 * M_PI - M_PI;
+    position_ = std::remainder(raw_position_, 2.0 * M_PI);
   } else {
-    position_ = raw_position * (2.0 * M_PI) / sensor_ticks_;
+    position_ = raw_position_;
   }
   double raw_velocity = talon_controller_->GetSelectedSensorVelocity();
   // Talons use d / 100ms as vel
-  velocity_ = raw_velocity * 10 * (2.0 * M_PI) / sensor_ticks_;
+  velocity_ = 10 * ticks_to_rads(raw_velocity);
 }
 
 int TalonSRXWrapper::get_load_enc() const {
   auto &sensor_collection = talon_controller_->GetSensorCollection();
-  int abs_ticks = 0;
   switch (load_sensor_) {
   case SensorType::PWM:
     return sensor_collection.GetPulseWidthPosition();
@@ -298,7 +292,7 @@ void TalonSRXWrapper::configure() {
   }
 
   talon_controller_->SetNeutralMode(NeutralMode::Brake);
-  talon_controller_->ConfigFeedbackNotContinuous(crossover_mode_);
+  talon_controller_->ConfigFeedbackNotContinuous(false);
   talon_controller_->ConfigSelectedFeedbackCoefficient(1.0);
   talon_controller_->EnableVoltageCompensation(true);
   talon_controller_->SetSensorPhase(invert_sensor_);

src/Teleop-Control/joystick_control/include/arm_teleop.hpp

@@ -31,15 +31,24 @@ class ArmTeleop : public rclcpp::Node {
   ~ArmTeleop();
 
 private:
-  enum ArmState { NONE = 0, MANUAL, IK, POS };
+  enum ArmState {
+    NO_MESSAGE = 0,
+    UNPLUG_ERROR,
+    WIGGLE_WARNING,
+    IDLE,
+    MANUAL,
+    IK,
+    POS
+  };
 
   void manual_arm_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg);
   void ik_arm_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg);
   void ik_pose_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg);
   void endeffector_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg);
   void clipboards_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg);
   void clear_dot();
-  bool check_initialized(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg);
+  ArmState
+  check_initialized(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg);
   bool moveit_servo_configure(const ArmState requested_state);
   ArmState requested_state(const std::vector<int32_t> &buttons) const;
   bool switch_states(const ArmState new_state);
@@ -50,6 +59,8 @@ class ArmTeleop : public rclcpp::Node {
   void run();
   void declareParameters();
   void loadParameters();
+  void publishState(const ArmState state);
+  bool is_ready(const ArmState state);
   static std::string state_to_string(const ArmState state);
 
   rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
@@ -71,8 +82,6 @@ class ArmTeleop : public rclcpp::Node {
 
   ArmState current_state_;
 
-  bool initialized_ = false;
-
   int kThrottleAxis;
   int kJoint1Axis;
   int kJoint2Axis;