Skip to content

Commit dd081db

Browse files
ayin21ayin21
authored
Teaching mode (#350)
2 parents e76a85e + 65d3e9c commit dd081db

File tree

10 files changed

+198
-90
lines changed

10 files changed

+198
-90
lines changed

Diff for: ‎bitbots_lowlevel/bitbots_buttons/CMakeLists.txt

+3-3
Original file line numberDiff line numberDiff line change
@@ -6,13 +6,13 @@ if(NOT CMAKE_CXX_STANDARD)
66
set(CMAKE_CXX_STANDARD 17)
77
endif()
88

9-
find_package(std_msgs REQUIRED)
109
find_package(ament_cmake REQUIRED)
1110
find_package(backward_ros REQUIRED)
1211
find_package(bitbots_docs REQUIRED)
1312
find_package(bitbots_msgs REQUIRED)
14-
find_package(bitbots_localization REQUIRED)
13+
find_package(game_controller_hl_interfaces REQUIRED)
1514
find_package(rclcpp REQUIRED)
15+
find_package(std_msgs REQUIRED)
1616
find_package(std_srvs REQUIRED)
1717
find_package(test_msgs REQUIRED)
1818

@@ -22,7 +22,7 @@ ament_target_dependencies(
2222
ament_cmake
2323
backward_ros
2424
bitbots_msgs
25-
bitbots_localization
25+
game_controller_hl_interfaces
2626
rclcpp
2727
std_msgs
2828
std_srvs

Diff for: ‎bitbots_lowlevel/bitbots_buttons/config/buttons.yaml

-1
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,6 @@
11
bitbots_buttons:
22
ros__parameters:
33
manual_penalty: True
4-
in_game: True
54
speak_active: True
65
short_time: 2.0 #[s]
76
debounce_time: 0.1 #[s]

Diff for: ‎bitbots_lowlevel/bitbots_buttons/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,8 @@
1919

2020
<depend>backward_ros</depend>
2121
<depend>bitbots_docs</depend>
22-
<depend>bitbots_localization</depend>
2322
<depend>bitbots_msgs</depend>
23+
<depend>game_controller_hl_interfaces</depend>
2424
<depend>rclcpp</depend>
2525
<depend>std_msgs</depend>
2626
<depend>std_srvs</depend>

Diff for: ‎bitbots_lowlevel/bitbots_buttons/src/button_node.cpp

+42-49
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,10 @@
11
#include <signal.h>
22

3-
#include <bitbots_localization/srv/reset_filter.hpp>
43
#include <bitbots_msgs/msg/audio.hpp>
54
#include <bitbots_msgs/msg/buttons.hpp>
65
#include <bitbots_msgs/srv/manual_penalize.hpp>
6+
#include <bitbots_msgs/srv/set_teaching_mode.hpp>
7+
#include <game_controller_hl_interfaces/msg/game_state.hpp>
78
#include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
89
#include <rclcpp/rclcpp.hpp>
910
#include <std_msgs/msg/bool.hpp>
@@ -21,38 +22,25 @@ class ButtonNode : public rclcpp::Node {
2122
this->declare_parameter<bool>("speak_active", true);
2223
this->declare_parameter<double>("short_time", 2.0);
2324
this->declare_parameter<bool>("manual_penalty", true);
24-
this->declare_parameter<bool>("in_game", true);
2525
this->declare_parameter<double>("debounce_time", 0.1);
2626
this->declare_parameter<bool>("speak", true);
2727

2828
this->get_parameter("speak_active", speaking_active_);
2929
this->get_parameter("short_time", short_time_);
3030
this->get_parameter("manual_penalty", manual_penalty_mode_);
31-
this->get_parameter("in_game", in_game_);
3231
this->get_parameter("debounce_time", debounce_time_);
3332
this->get_parameter("speak", speak_);
3433

35-
// --- Class variables ---
36-
button1_ = false;
37-
button2_ = false;
38-
button3_ = false;
39-
button1_time_ = 0.0;
40-
button2_time_ = 0.0;
41-
button3_time_ = 0.0;
42-
4334
// --- Initialize Topics ---
4435
speak_pub_ = this->create_publisher<bitbots_msgs::msg::Audio>("/speak", 1);
45-
shoot_publisher_ = this->create_publisher<std_msgs::msg::Bool>("/shoot_button", 1);
4636

4737
if (manual_penalty_mode_) {
4838
manual_penalize_client_ = this->create_client<bitbots_msgs::srv::ManualPenalize>("manual_penalize");
4939
manual_penalty_mode_ = manual_penalize_client_->wait_for_service(3s);
5040
}
5141

52-
if (!in_game_) {
53-
foot_zero_client_ = this->create_client<std_srvs::srv::Empty>("set_foot_zero");
54-
foot_zero_available_ = foot_zero_client_->wait_for_service(3s);
55-
}
42+
foot_zero_client_ = this->create_client<std_srvs::srv::Empty>("set_foot_zero");
43+
foot_zero_available_ = foot_zero_client_->wait_for_service(3s);
5644

5745
power_client_ = this->create_client<std_srvs::srv::SetBool>("/core/switch_power");
5846
while (!power_client_->wait_for_service(3s)) {
@@ -63,12 +51,16 @@ class ButtonNode : public rclcpp::Node {
6351
RCLCPP_INFO(this->get_logger(), "service switch_power not available, waiting again...");
6452
}
6553

66-
localization_client_ = this->create_client<bitbots_localization::srv::ResetFilter>("reset_localization");
67-
localization_available_ = localization_client_->wait_for_service(3s);
54+
teaching_mode_client_ = this->create_client<bitbots_msgs::srv::SetTeachingMode>("set_teaching_mode");
6855
buttons_sub_ = this->create_subscription<bitbots_msgs::msg::Buttons>(
6956
"/buttons", 1, std::bind(&bitbots_buttons::ButtonNode::buttonCb, this, _1));
57+
gamestate_sub_ = this->create_subscription<game_controller_hl_interfaces::msg::GameState>(
58+
"gamestate", 1, std::bind(&bitbots_buttons::ButtonNode::gamestateCb, this, _1));
7059
}
7160

61+
// Sets the in_game_ variable to true, if a Gamestate message from the Gamecontroller arrives.
62+
void gamestateCb(const game_controller_hl_interfaces::msg::GameState::SharedPtr msg) { in_game_ = true; }
63+
7264
void buttonCb(const bitbots_msgs::msg::Buttons::SharedPtr msg) {
7365
// button1 - red
7466
// button2 - green
@@ -90,12 +82,11 @@ class ButtonNode : public rclcpp::Node {
9082
if (current_time - button1_time_ > debounce_time_) {
9183
if (current_time - button1_time_ < short_time_ || in_game_) {
9284
// button 1 short
93-
speak("1 short");
94-
setPower(false);
85+
speak("Red button pressed short. Turning motor power off.");
86+
setPower(false); // this can not be reversed because the button cuts the power of himself
9587
} else {
9688
// button 1 long
97-
speak("1 long");
98-
setPower(true);
89+
speak("Red button pressed long. No action implemented.");
9990
}
10091
}
10192
button1_time_ = 0;
@@ -105,12 +96,12 @@ class ButtonNode : public rclcpp::Node {
10596
double current_time = this->get_clock()->now().seconds();
10697
if (current_time - button2_time_ > debounce_time_) {
10798
if (current_time - button2_time_ < short_time_ || in_game_) {
108-
speak("2 short");
99+
speak("Green button pressed short. Try deactivating Penalty mode");
109100
setPenalty(false);
110-
resetLocalization();
111101
} else {
112-
speak("2 long");
113-
setPenalty(false);
102+
speak("Green button pressed long. Try deactivating teaching mode");
103+
// Turn teaching mode off
104+
setTeachingMode(false);
114105
}
115106
}
116107
button2_time_ = 0;
@@ -120,17 +111,30 @@ class ButtonNode : public rclcpp::Node {
120111
double current_time = this->get_clock()->now().seconds();
121112
if (current_time - button3_time_ > debounce_time_) {
122113
if (current_time - button3_time_ < short_time_ || in_game_) {
123-
speak("3 short");
114+
speak("Blue button pressed short. Try activating Penalty mode");
124115
setPenalty(true);
125116
} else {
126-
speak("3 long");
127-
setPenalty(true);
117+
speak("Blue button pressed long. Try switching teaching mode state");
118+
// Turn teaching mode on or switch between HOLD and TEACH
119+
setTeachingMode(true);
128120
}
129121
}
130122
button3_time_ = 0;
131123
}
132124
}
133125

126+
void setTeachingMode(bool state) {
127+
auto request = std::make_shared<bitbots_msgs::srv::SetTeachingMode::Request>();
128+
if (state) {
129+
// switch teaching mode state to SWITCH
130+
request->state = bitbots_msgs::srv::SetTeachingMode::Request::SWITCH;
131+
} else {
132+
// switch teaching mode state to OFF
133+
request->state = bitbots_msgs::srv::SetTeachingMode::Request::OFF;
134+
}
135+
teaching_mode_client_->async_send_request(request);
136+
}
137+
134138
void setPenalty(bool penalize) {
135139
// Penalizes the robot, if it is not penalized and manual penalty mode is true.
136140
if (manual_penalty_mode_) {
@@ -160,16 +164,6 @@ class ButtonNode : public rclcpp::Node {
160164
power_client_->async_send_request(request);
161165
}
162166

163-
void resetLocalization() {
164-
if (localization_available_) {
165-
auto request = std::make_shared<bitbots_localization::srv::ResetFilter::Request>();
166-
request->init_mode = 0;
167-
localization_client_->async_send_request(request);
168-
} else {
169-
RCLCPP_WARN(this->get_logger(), "service not available");
170-
}
171-
}
172-
173167
private:
174168
void speak(const std::string& text) {
175169
/**
@@ -186,26 +180,25 @@ class ButtonNode : public rclcpp::Node {
186180
bool speaking_active_;
187181
double short_time_;
188182
bool manual_penalty_mode_;
189-
bool in_game_;
183+
bool in_game_ = false;
190184
double debounce_time_;
191185
bool speak_;
192186

193-
bool button1_;
194-
bool button2_;
195-
bool button3_;
196-
bool localization_available_;
187+
bool button1_ = false;
188+
bool button2_ = false;
189+
bool button3_ = false;
197190
bool foot_zero_available_;
198-
double button1_time_;
199-
double button2_time_;
200-
double button3_time_;
191+
double button1_time_ = 0.0;
192+
double button2_time_ = 0.0;
193+
double button3_time_ = 0.0;
201194

202195
rclcpp::Publisher<bitbots_msgs::msg::Audio>::SharedPtr speak_pub_;
203-
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr shoot_publisher_;
204196
rclcpp::Client<bitbots_msgs::srv::ManualPenalize>::SharedPtr manual_penalize_client_;
197+
rclcpp::Client<bitbots_msgs::srv::SetTeachingMode>::SharedPtr teaching_mode_client_;
205198
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr foot_zero_client_;
206199
rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr power_client_;
207-
rclcpp::Client<bitbots_localization::srv::ResetFilter>::SharedPtr localization_client_;
208200
rclcpp::Subscription<bitbots_msgs::msg::Buttons>::SharedPtr buttons_sub_;
201+
rclcpp::Subscription<game_controller_hl_interfaces::msg::GameState>::SharedPtr gamestate_sub_;
209202
};
210203
} // namespace bitbots_buttons
211204

Diff for: ‎bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/change_motor_torque.py

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
from bitbots_hcm.hcm_dsd.actions import AbstractHCMActionElement
2+
from bitbots_msgs.msg import JointTorque
3+
4+
5+
class SetTorque(AbstractHCMActionElement):
6+
def __init__(self, blackboard, dsd, parameters):
7+
super().__init__(blackboard, dsd, parameters)
8+
self.stiff = parameters.get("stiff", True)
9+
10+
def perform(self, reevaluate=False):
11+
if self.blackboard.current_joint_state is None:
12+
self.blackboard.node.get_logger().warning(
13+
"Cannot set joint stiffness for teaching mode because no joint states where received!"
14+
)
15+
return self.pop()
16+
17+
self.blackboard.torque_publisher.publish(
18+
JointTorque(
19+
joint_names=self.blackboard.current_joint_state.name,
20+
on=[self.stiff] * len(self.blackboard.current_joint_state.name),
21+
)
22+
)
23+
return self.pop()

Diff for: ‎bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/teaching.py

+35
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement
2+
from bitbots_msgs.srv import SetTeachingMode
3+
4+
5+
class TeachingMode(AbstractHCMDecisionElement):
6+
"""
7+
Decides if the robot is currently in teaching mode.
8+
In the teaching mode the robot can be puppeteered freely.
9+
To do this we deactivate the torque on all motors.
10+
If we leave the teaching mode we activate the torque again.
11+
"""
12+
13+
def __init__(self, blackboard, dsd, parameters):
14+
super().__init__(blackboard, dsd, parameters)
15+
self.last_state_on = False
16+
17+
def perform(self, reevaluate=False):
18+
if self.blackboard.teaching_mode_state == SetTeachingMode.Request.TEACH:
19+
self.last_state_on = True
20+
# We activated the teaching mode
21+
return "TEACH"
22+
elif self.blackboard.teaching_mode_state == SetTeachingMode.Request.HOLD:
23+
# We want to hold the pose
24+
self.last_state_on = True
25+
return "HOLD"
26+
elif self.last_state_on:
27+
self.last_state_on = False
28+
# We just deactivated the teaching mode and need to clean up
29+
return "FINISHED"
30+
else:
31+
# We are not in the teaching mode
32+
return "OFF"
33+
34+
def get_reevaluate(self):
35+
return True

Diff for: ‎bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd

+38-34
Original file line numberDiff line numberDiff line change
@@ -1,37 +1,41 @@
11
-->HCM
22
$StartHCM
33
START_UP --> @RobotStateStartup, @PlayAnimationDynup + direction:walkready
4-
RUNNING --> $Stop
5-
STOPPED --> @CancelGoals, @StopWalking, @PlayAnimationDynup + direction:walkready, @Wait
6-
FREE -->$RecordAnimation
7-
RECORD_ACTIVE --> @RobotStateRecord, @Wait
8-
FREE --> $CheckMotors
9-
MOTORS_NOT_STARTED --> @RobotStateStartup, @WaitForMotorStartup
10-
OVERLOAD --> @RobotStateMotorOff, @CancelGoals, @StopWalking, @PlayAnimationFallingFront, @TurnMotorsOff, @Wait
11-
PROBLEM --> @RobotStateHardwareProblem, @WaitForMotors
12-
TURN_ON --> @TurnMotorsOn, @PlayAnimationDynup + direction:walkready, @Wait
13-
OKAY --> $CheckIMU
14-
IMU_NOT_STARTED --> @RobotStateStartup, @WaitForIMUStartup
15-
PROBLEM --> @RobotStateHardwareProblem, @WaitForIMU
16-
OKAY --> $CheckPressureSensor
17-
PRESSURE_NOT_STARTED --> @RobotStateStartup, @WaitForPressureStartup
18-
PROBLEM --> @RobotStateHardwareProblem, @WaitForPressure
19-
OKAY --> $PickedUp
20-
PICKED_UP --> @RobotStatePickedUp, @PlayAnimationDynup + direction:walkready, @Wait
21-
ON_GROUND --> $Fallen
22-
FALLEN_FRONT --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @PlayAnimationDynup + direction:front
23-
FALLEN_BACK --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @SetFootZero, @PlayAnimationDynup + direction:back
24-
FALLEN_RIGHT --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningBackRight
25-
FALLEN_LEFT --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningBackLeft
26-
NOT_FALLEN --> $Falling
27-
FALLING_LEFT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingLeft, @Wait
28-
FALLING_RIGHT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingRight, @Wait
29-
FALLING_FRONT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingFront, @Wait
30-
FALLING_BACK --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingBack, @Wait
31-
NOT_FALLING --> $PlayingExternalAnimation
32-
ANIMATION_RUNNING --> @StopWalking, @RobotStateAnimationRunning, @Wait
33-
FREE --> $RecentWalkingGoals
34-
STAY_WALKING --> @RobotStateWalking, @Wait
35-
NOT_WALKING --> $RecentKickGoals
36-
KICKING --> @RobotStateKicking, @Wait
37-
NOT_KICKING --> @RobotStateControllable, @Wait
4+
RUNNING --> $RecordAnimation
5+
RECORD_ACTIVE --> @RobotStateRecord, @Wait
6+
FREE --> $CheckMotors
7+
MOTORS_NOT_STARTED --> @RobotStateStartup, @WaitForMotorStartup
8+
OVERLOAD --> @RobotStateMotorOff, @CancelGoals, @StopWalking, @PlayAnimationFallingFront, @TurnMotorsOff, @Wait
9+
PROBLEM --> @RobotStateHardwareProblem, @WaitForMotors
10+
TURN_ON --> @TurnMotorsOn, @PlayAnimationDynup + direction:walkready, @Wait
11+
OKAY --> $TeachingMode
12+
TEACH --> @RobotStateRecord, @SetTorque + stiff:false, @Wait
13+
HOLD --> @SetTorque + stiff:true, @Wait
14+
FINISHED --> @SetTorque + stiff:true + r:false, @RobotStateControllable, @PlayAnimationDynup + direction:walkready
15+
OFF --> $Stop
16+
STOPPED --> @CancelGoals, @StopWalking, @PlayAnimationDynup + direction:walkready, @Wait
17+
FREE -->$CheckIMU
18+
IMU_NOT_STARTED --> @RobotStateStartup, @WaitForIMUStartup
19+
PROBLEM --> @RobotStateHardwareProblem, @WaitForIMU
20+
OKAY --> $CheckPressureSensor
21+
PRESSURE_NOT_STARTED --> @RobotStateStartup, @WaitForPressureStartup
22+
PROBLEM --> @RobotStateHardwareProblem, @WaitForPressure
23+
OKAY --> $PickedUp
24+
PICKED_UP --> @RobotStatePickedUp, @PlayAnimationDynup + direction:walkready, @Wait
25+
ON_GROUND --> $Fallen
26+
FALLEN_FRONT --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @PlayAnimationDynup + direction:front
27+
FALLEN_BACK --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @SetFootZero, @PlayAnimationDynup + direction:back
28+
FALLEN_RIGHT --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningBackRight
29+
FALLEN_LEFT --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningBackLeft
30+
NOT_FALLEN --> $Falling
31+
FALLING_LEFT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingLeft, @Wait
32+
FALLING_RIGHT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingRight, @Wait
33+
FALLING_FRONT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingFront, @Wait
34+
FALLING_BACK --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingBack, @Wait
35+
NOT_FALLING --> $PlayingExternalAnimation
36+
ANIMATION_RUNNING --> @StopWalking, @RobotStateAnimationRunning, @Wait
37+
FREE --> $RecentWalkingGoals
38+
STAY_WALKING --> @RobotStateWalking, @Wait
39+
NOT_WALKING --> $RecentKickGoals
40+
KICKING --> @RobotStateKicking, @Wait
41+
NOT_KICKING --> @RobotStateControllable, @Wait

0 commit comments

Comments
 (0)