diff --git a/src/Arm/arm_control/config/arm_config.yaml b/src/Arm/arm_control/config/arm_config.yaml index 83b91a99..2ed3f730 100644 --- a/src/Arm/arm_control/config/arm_config.yaml +++ b/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 diff --git a/src/Arm/arm_control/include/arm_control/MoveGroupNode.hpp b/src/Arm/arm_control/include/arm_control/MoveGroupNode.hpp index 0f0e90ce..f7460159 100644 --- a/src/Arm/arm_control/include/arm_control/MoveGroupNode.hpp +++ b/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 request, std::shared_ptr response); + void + stopCallback(const std::shared_ptr request, + std::shared_ptr response); + double planning_time_; int planning_attempts_; double vel_scaling_; @@ -119,6 +124,8 @@ class MoveGroupNode : public rclcpp::Node { rclcpp::Service::SharedPtr go_to_cam_coord_service_; + rclcpp::Service::SharedPtr stop_service_; + rclcpp::TimerBase::SharedPtr initialization_timer_; }; diff --git a/src/Arm/arm_control/src/MoveGroupNode.cpp b/src/Arm/arm_control/src/MoveGroupNode.cpp index 51e1a48a..13f227ce 100644 --- a/src/Arm/arm_control/src/MoveGroupNode.cpp +++ b/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( + "~/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 request, + std::shared_ptr response) { + (void)request; + + std::lock_guard 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) \ No newline at end of file diff --git a/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp b/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp index ebff13d3..a0f30131 100644 --- a/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp +++ b/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_; diff --git a/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp index bf46fb68..2997123e 100644 --- a/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp +++ b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp @@ -94,8 +94,7 @@ TalonSRXWrapper::TalonSRXWrapper(const hardware_interface::ComponentInfo &joint, debug_pub_ = debug_node_->create_publisher( joint.name + "/status", rclcpp::SystemDefaultsQoS()); - sensor_offset_ticks_ = - static_cast(sensor_offset * sensor_ticks_ / (2.0 * M_PI)); + sensor_offset_ticks_ = static_cast(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,37 +186,29 @@ 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, @@ -217,26 +216,21 @@ void TalonSRXWrapper::write() { } void TalonSRXWrapper::read() { - int raw_position = - talon_controller_->GetSelectedSensorPosition() - sensor_offset_ticks_; + const int raw_ticks = talon_controller_->GetSelectedSensorPosition() - + static_cast(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(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_); diff --git a/src/Teleop-Control/joystick_control/include/arm_teleop.hpp b/src/Teleop-Control/joystick_control/include/arm_teleop.hpp index ffc80dc8..de9c99e2 100644 --- a/src/Teleop-Control/joystick_control/include/arm_teleop.hpp +++ b/src/Teleop-Control/joystick_control/include/arm_teleop.hpp @@ -31,7 +31,15 @@ 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 joystickMsg); void ik_arm_control(std::shared_ptr joystickMsg); @@ -39,7 +47,8 @@ class ArmTeleop : public rclcpp::Node { void endeffector_control(std::shared_ptr joystickMsg); void clipboards_control(std::shared_ptr joystickMsg); void clear_dot(); - bool check_initialized(std::shared_ptr joystickMsg); + ArmState + check_initialized(std::shared_ptr joystickMsg); bool moveit_servo_configure(const ArmState requested_state); ArmState requested_state(const std::vector &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::SharedPtr joy_sub_; @@ -71,8 +82,6 @@ class ArmTeleop : public rclcpp::Node { ArmState current_state_; - bool initialized_ = false; - int kThrottleAxis; int kJoint1Axis; int kJoint2Axis; diff --git a/src/Teleop-Control/joystick_control/src/arm_teleop.cpp b/src/Teleop-Control/joystick_control/src/arm_teleop.cpp index 901f34a6..c375b8ce 100644 --- a/src/Teleop-Control/joystick_control/src/arm_teleop.cpp +++ b/src/Teleop-Control/joystick_control/src/arm_teleop.cpp @@ -10,7 +10,7 @@ ArmTeleop::ArmTeleop(const rclcpp::NodeOptions &options) : Node("arm_node", options) { declareParameters(); loadParameters(); - current_state_ = NONE; + current_state_ = NO_MESSAGE; joy_sub_ = this->create_subscription( "/joy", rclcpp::QoS(2).best_effort(), [this](const sensor_msgs::msg::Joy::SharedPtr msg) { @@ -189,7 +189,7 @@ void ArmTeleop::endeffector_control( } bool ArmTeleop::moveit_servo_configure(const ArmState requested_state) { - if (requested_state == NONE) { + if (requested_state == IDLE) { return true; } if (!servo_input_client_->wait_for_service(std::chrono::seconds(2))) { @@ -228,14 +228,20 @@ bool ArmTeleop::moveit_servo_configure(const ArmState requested_state) { std::string ArmTeleop::state_to_string(const ArmState state) { switch (state) { + case ArmState::NO_MESSAGE: + return "Waiting"; + case ArmState::UNPLUG_ERROR: + return "Error"; + case ArmState::WIGGLE_WARNING: + return "Warning"; + case ArmState::IDLE: + return "Idle"; case ArmState::MANUAL: return "Manual"; case ArmState::IK: return "Cartesian IK"; case ArmState::POS: return "Visual Position"; - case ArmState::NONE: - return "Disabled"; default: return "Unknown"; } @@ -246,14 +252,12 @@ bool ArmTeleop::switch_states(const ArmState requested_state) { return false; } - if (current_state_ == NONE) { + if (current_state_ == IDLE) { stop_move_group_motion(); } current_state_ = requested_state; - auto msg = std_msgs::msg::String(); - msg.data = state_to_string(current_state_); - state_pub_->publish(msg); + publishState(current_state_); return true; } @@ -266,27 +270,40 @@ void ArmTeleop::clear_dot() { targetPositionY = kCamHeight / 2; } -bool ArmTeleop::check_initialized( +ArmTeleop::ArmState ArmTeleop::check_initialized( const sensor_msgs::msg::Joy::SharedPtr joystickMsg) { if (std::abs(joystickMsg->axes[kJoint1Axis]) < 0.01 && std::abs(joystickMsg->axes[kJoint2Axis]) < 0.01 && std::abs(joystickMsg->axes[kJoint3Axis]) < 0.01 && std::abs(joystickMsg->axes[kJoint4Axis]) < 0.01 && std::abs(joystickMsg->axes[kJoint6Axis]) < 0.01) { - return true; - } - - RCLCPP_WARN_THROTTLE( + return IDLE; + } + // Wiggle warning has values that are 1 on joints 1, 2, and 4 + // When each axes is moved even a bit, it reads properly + if ((std::abs(joystickMsg->axes[kJoint1Axis]) > 0.99 || + std::abs(joystickMsg->axes[kJoint2Axis]) > 0.99 || + std::abs(joystickMsg->axes[kJoint4Axis]) > 0.99)) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *(this->get_clock()), 1000, + "Arm Controller reading non-zero values on joystick axes. " + "Please wiggle the joysticks to initialize. (Throttled to 1s)"); + return WIGGLE_WARNING; + } + // Other errors probably mean the joystick is in a bad state and needs to be + // unplugged and replugged + RCLCPP_ERROR_THROTTLE( this->get_logger(), *(this->get_clock()), 1000, - "Arm Controller not reading zeros on joystick axes. " - "Please center joysticks to initialize. (Throttled to 1s)"); - return false; + "Arm Controller reading non-zero values on joystick axes. " + "Please unplug and replug the joystick. " + "(Throttled to 1s)"); + return UNPLUG_ERROR; } ArmTeleop::ArmState ArmTeleop::requested_state(const std::vector &buttons) const { if (buttons[kDisableButton]) { - return NONE; + return IDLE; } if (buttons[kIkButton]) { @@ -304,7 +321,30 @@ ArmTeleop::requested_state(const std::vector &buttons) const { return current_state_; } +bool ArmTeleop::is_ready(const ArmState state) { + if (state == NO_MESSAGE) { + return false; + } + + if (state == UNPLUG_ERROR) { + return false; + } + + if (state == WIGGLE_WARNING) { + return false; + } + + return true; +} + +void ArmTeleop::publishState(const ArmState state) { + auto msg = std_msgs::msg::String(); + msg.data = state_to_string(state); + state_pub_->publish(msg); +} + void ArmTeleop::run() { + publishState(current_state_); while (true) { std::shared_ptr joystickMsg; { @@ -316,10 +356,13 @@ void ArmTeleop::run() { joystickMsg = curr_msg_; curr_msg_.reset(); } - - if (!initialized_) { - initialized_ = check_initialized(joystickMsg); - last_msg_ = joystickMsg; + if (!is_ready(current_state_)) { + const auto new_state = check_initialized(joystickMsg); + if (new_state == current_state_) { + continue; + } + current_state_ = new_state; + publishState(current_state_); continue; } @@ -361,7 +404,7 @@ void ArmTeleop::manual_arm_control( auto &axes = joystickMsg->axes; auto &buttons = joystickMsg->buttons; - joint_msg.header.stamp = joystickMsg->header.stamp; + joint_msg.header.stamp = now(); joint_msg.velocities = {-axes[kJoint1Axis], -axes[kJoint2Axis], @@ -370,7 +413,11 @@ void ArmTeleop::manual_arm_control( -static_cast(buttons[kWristYaw_positive] - buttons[kWristYaw_negative]), -axes[kJoint6Axis]}; - + // Map throttle from [-1, 1] to [0, 1] + const auto throttle = (axes[kThrottleAxis] + 1.0) / 2.0; + for (auto &vel : joint_msg.velocities) { + vel *= throttle; + } joint_pub_->publish(joint_msg); } @@ -383,15 +430,16 @@ void ArmTeleop::ik_arm_control( auto &axes = joystickMsg->axes; auto &buttons = joystickMsg->buttons; - twist_msg.twist.linear.x = axes[kJoint2Axis]; - twist_msg.twist.linear.y = axes[kJoint1Axis]; - twist_msg.twist.linear.z = axes[kJoint3Axis]; - twist_msg.twist.angular.x = axes[kJoint4Axis]; - twist_msg.twist.angular.y = axes[kJoint6Axis]; - twist_msg.twist.angular.z = - (static_cast(buttons[kWristYaw_positive] - - buttons[kWristYaw_negative])) * - axes[kThrottleAxis]; + const auto throttle = (axes[kThrottleAxis] + 1.0) / 2.0; + + twist_msg.twist.linear.x = axes[kJoint3Axis] * throttle; + twist_msg.twist.linear.y = axes[kJoint1Axis] * throttle; + twist_msg.twist.linear.z = axes[kJoint2Axis] * throttle; + twist_msg.twist.angular.x = -axes[kJoint4Axis] * throttle; + twist_msg.twist.angular.y = static_cast(buttons[kWristYaw_positive] - + buttons[kWristYaw_negative]) * + throttle; + twist_msg.twist.angular.z = -axes[kJoint6Axis] * throttle; ik_pub_->publish(twist_msg); } diff --git a/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro index 1c9e6986..493c4e74 100644 --- a/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro +++ b/src/URDF/rover_urdf/urdf/arm_urdf.ros2_control.xacro @@ -79,7 +79,7 @@ quadrature 2039808 0.0 - true + true true can1 true