Add Extended State Feedback: Effort Reporting & Object Detection - #128
Add Extended State Feedback: Effort Reporting & Object Detection#128Amineki6 wants to merge 2 commits into
Conversation
peter-mitrano-ar
left a comment
peter-mitrano-ar
left a comment
There was a problem hiding this comment.
Great start! I'm sure we can get this merged, but I apologize in advance if I am slow to respond. But from testing the basic functionality is mostly there!
I tried running the robotic_control launch file, and I get this:
[ros2_control_node-1] [WARN]: Unable to activate controller 'robotiq_gripper_controller' since the command interface 'robotiq_85_left_knuckle_joint/effort' is not available.
Which makes sense, because it's called set_gripper_max_effort, not effort. Same for velocity. But I was able to fix that in the robotiq_controllers.yaml, so I can push that if you want. In fact, if I have permission, I can push fixes for some of the other commends I made.
However, the output velocity differentiation is not really usable. See the attached plots.
As you can see, the one-step finite difference is incredibly noisy.
Also, I see messages like this, even with the main branch:
[ros2_control_node-1] [WARN]: Overrun detected! The controller manager missed its desired rate of 200 Hz. The loop took 5.797472 ms (missed cycles : 2).
Which is quite concerning, though maybe it's just my set up is weird. Did you test this on real hardware? which ROS version?
|
|
||
| uint8_t DefaultDriver::get_object_detection_status() | ||
| { | ||
| return static_cast<uint8_t>(object_detection_status_); |
There was a problem hiding this comment.
Are we missing actually setting this variable? I can't find it... thought somehow it seems to be working
|
|
||
| // Read parameters. | ||
| gripper_closed_pos_ = stod(info_.hardware_parameters.at("gripper_closed_position")); | ||
| gripper_closed_pos_ = stod(info_.hardware_parameters["gripper_closed_position"]); |
There was a problem hiding this comment.
why switch to [? using at seems safer, because it wil throw an exception instead of creating an default-initialized double value?
| if (joint.state_interfaces.size() != 2 && joint.state_interfaces.size() != 3 && joint.state_interfaces.size() != 4) | ||
| { | ||
| RCLCPP_FATAL(kLogger, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), | ||
| RCLCPP_FATAL(kLogger, "Joint '%s' has %zu state interface. 2, 3 or 4 expected.", joint.name.c_str(), | ||
| joint.state_interfaces.size()); | ||
| return CallbackReturn::ERROR; | ||
| } | ||
|
|
||
| for (int i = 0; i < 2; ++i) | ||
| for (size_t i = 0; i < joint.state_interfaces.size(); ++i) | ||
| { | ||
| if (!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION || | ||
| joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY)) | ||
| joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY || | ||
| joint.state_interfaces[i].name == hardware_interface::HW_IF_EFFORT || | ||
| joint.state_interfaces[i].name == "object_detection_status")) |
There was a problem hiding this comment.
Is there a good reason to still consider 2 or 3 interfaces? isn't it always 4?
| if (first_run || current_speed != last_speed || current_force != last_force || | ||
| current_position != last_position) | ||
| { | ||
| this->driver_->set_speed(current_speed); | ||
| this->driver_->set_force(current_force); | ||
| this->driver_->set_gripper_position(current_position); | ||
|
|
||
| last_speed = current_speed; | ||
| last_force = current_force; | ||
| last_position = current_position; | ||
| first_run = false; |
There was a problem hiding this comment.
What's the motivation for avoid redundant writes? It adds some complexity to the code, and I'm not sure if it has any effect.
|
|
||
| hardware_interface::CallbackReturn | ||
| RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareComponentInterfaceParams& params) | ||
| hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info) |
There was a problem hiding this comment.
HardwareInfo is deprecated, for me, at least on jazzy. So I think we should revert this.
2 participants
This PR replaces #119 , which was accidentally closed due to a repository visibility update. The changes remain identical.