Placing server 2023 #212
Placing server 2023 #212
Conversation
- fourber: use linear position (now published from controller) instead of comparing angular position from talon to linear position from config - add mid safety zone for when the carraige is fully within the collision zone - elevater: publish SAFETY_TO_NO_SAFETY after elevator moves, not before just tested in simulation and it seems to work well, may have missed some edge cases though
| safetyBoundsAndCallService(safety_high_distance_, safety_high_below_); | ||
| safety_set = true; | ||
| FourberINFO("High"); | ||
| safety_state_lock_.lock(); |
There was a problem hiding this comment.
Replace with a lock_guard or unique_lock
| safety_set = true; | ||
| FourberINFO("Mid"); | ||
| safety_state_lock_.lock(); | ||
| safety_state_.min_distance_above = std::max(safety_state_.min_distance_above, safety_mid_distance_above_); |
There was a problem hiding this comment.
Could make an updateAbove method in SafetyState which does this.
Or even an updateMinDistance which takes the safety_mid_distance_above_, safety_mid_distance_below_ values.
Or even better, if safety_mid_distance_above_ and safety_mid_distance_below_ were stored in their own SafetyState object, the UpdateMinDistance() method could just take that other SafetyState object.
There was a problem hiding this comment.
And at that point, you could move the mutex into a member var of SafetyState and all of these cases would be one-line calls
| double safety_low_distance_; | ||
| bool safety_high_below_; | ||
| bool safety_low_below_; | ||
| double safety_high_distance_above_; |
There was a problem hiding this comment.
You have a struct which holds above/below pairs, can that be used to hold these values as well? See below for ideas on how it makes the code cleaner
| } | ||
| game_piece_sub_ = nh_.subscribe("/game_piece", 1, &PlacingServer2023::gamePieceCallback, this); | ||
| game_piece_position_sub_ = nh_.subscribe("/game_piece_position", 1, &PlacingServer2023::gamePiecePositionCallback, this); | ||
| const std::map<std::string, std::string> service_connection_header{{"tcp_nodelay", "1"}}; |
|
|
||
| ros::Time timeout_time = ros::Time::now() + timeout; | ||
|
|
||
| ros::Rate r(100); |
There was a problem hiding this comment.
100hz seems a bit high for simply polling for a mechanism to be in the right place?
|
|
||
| behavior_actions::Fourber2023Goal fourber_goal; | ||
| fourber_goal.piece = latest_game_piece_; | ||
| fourber_goal.mode = goal->node + 1; // please don't change the actionlib files |
There was a problem hiding this comment.
This is going to break at some point. Probably best to do an explicit conversion. Shouldn't be too bad, just a switch statement checking input and setting output
There was a problem hiding this comment.
Make it a function so it can be reused below.
And add in a default for cases which don't match so this can be easily debugged.
| @@ -95,8 +95,7 @@ class ElevatorController_2023 : public controller_interface::MultiInterfaceContr | |||
| }; //class | |||
|
|
|||
|
|
|||
| //namespace | |||
| #endif | |||
| //namespac | |||
| @@ -86,6 +89,31 @@ class FourBarController_2023 : public controller_interface::MultiInterfaceContro | |||
| double angle = acos((min_extension_ - intake_length_ - parallel_bar_length_) / diagonal_bar_length_) - xAngle; | |||
| return below ? acos((min_extension_ - intake_length_ - parallel_bar_length_) / diagonal_bar_length_) + xAngle : angle; | |||
| } | |||
|
|
|||
| std::pair<double, bool> stateFromAngle(double angle) const | |||
There was a problem hiding this comment.
I'd prefer a struct or class with named members, makes it easier to follow. And you might be able to move some of the math or at least the conversion to the msg into a method
| @@ -43,6 +43,7 @@ hardware_interface: | |||
|
|
|||
| - {name: four_bar_leader, type: can_talon_fx, can_bus: CANivore, can_id: 31, local: true} | |||
| - {name: intake_leader, type: can_talon_fx, can_bus: CANivore, can_id: 41, local: true} | |||
| - {name: elevator_master, type: can_talon_fx, can_bus: CANivore, can_id: 51, local: true} | |||
There was a problem hiding this comment.
Make this elevator_leader to match the other joints
Might as well add the elevator_follower as well
And then update can ids to match the robot
elevator is 31, 32
four bar is 41
intake is 51
Tested with everything except aligning to game pieces (loading motor simulation + stage at the same time is hard, and the game piece state server isn't finished yet)