Skip to content

Commit 865650d

Browse files
YoujianWuYoujianWu
authored
Merge pull request #126 from YoujianWu/legged_manual_new
Add legged_balance manual
2 parents 13ab20f + c95888d commit 865650d

File tree

7 files changed

+370
-97
lines changed

7 files changed

+370
-97
lines changed

‎include/rm_manual/balance_manual.h‎

Lines changed: 3 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
//
2-
// Created by yuchen on 2023/4/3.
2+
// Created by kook on 9/28/24.
33
//
44

55
#pragma once
@@ -15,6 +15,8 @@ class BalanceManual : public ChassisGimbalShooterCoverManual
1515

1616
protected:
1717
void updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
18+
void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
19+
1820
void wPress() override;
1921
void sPress() override;
2022
void aPress() override;
@@ -26,28 +28,13 @@ class BalanceManual : public ChassisGimbalShooterCoverManual
2628
void aPressing() override;
2729
void sPressing() override;
2830
void dPressing() override;
29-
void bPress() override;
30-
void vPress() override;
3131
void ctrlZPress() override;
3232
void rightSwitchDownRise() override;
3333
void rightSwitchMidRise() override;
34-
3534
void sendCommand(const ros::Time& time) override;
36-
void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
37-
void ctrlXPress();
38-
void modeFallen(ros::Duration duration);
39-
void modeNormalize();
40-
rm_common::BalanceCommandSender* balance_cmd_sender_{};
4135

4236
private:
43-
void balanceStateCallback(const rm_msgs::BalanceState::ConstPtr& msg);
44-
45-
ros::Subscriber state_sub_;
46-
double balance_dangerous_angle_;
47-
4837
bool flank_ = false, reverse_ = false;
4938
std::string flank_frame_, reverse_frame_;
50-
51-
InputEvent v_event_, ctrl_x_event_, auto_fallen_event_;
5239
};
5340
} // namespace rm_manual

‎include/rm_manual/legged_wheel_balance_manual.h‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
//
2+
// Created by kook on 9/28/24.
3+
//
4+
5+
#pragma once
6+
7+
#include "rm_manual/balance_manual.h"
8+
9+
namespace rm_manual
10+
{
11+
class LeggedWheelBalanceManual : public BalanceManual
12+
{
13+
public:
14+
LeggedWheelBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee);
15+
16+
protected:
17+
void updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
18+
void shiftPress() override;
19+
void shiftRelease() override;
20+
void bPress() override;
21+
void bRelease() override;
22+
void ctrlZPress() override;
23+
void rightSwitchDownRise() override;
24+
void rightSwitchMidRise() override;
25+
void ctrlPress();
26+
void ctrlRelease();
27+
28+
void sendCommand(const ros::Time& time) override;
29+
void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
30+
void ctrlGPress();
31+
rm_common::LegCommandSender* legCommandSender_{};
32+
33+
private:
34+
bool stretch_ = false, stretching_ = false, is_increasing_length_ = false;
35+
InputEvent ctrl_event_, ctrl_g_event_;
36+
ros::Subscriber unstick_sub_;
37+
void unstickCallback(const std_msgs::BoolConstPtr& msg);
38+
};
39+
} // namespace rm_manual

‎include/rm_manual/wheeled_balance_manual.h‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
//
2+
// Created by yuchen on 2023/4/3.
3+
//
4+
5+
#pragma once
6+
7+
#include "rm_manual/balance_manual.h"
8+
9+
namespace rm_manual
10+
{
11+
class WheeledBalanceManual : public BalanceManual
12+
{
13+
public:
14+
WheeledBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee);
15+
16+
protected:
17+
void sendCommand(const ros::Time& time) override;
18+
void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
19+
void rightSwitchDownRise() override;
20+
void rightSwitchMidRise() override;
21+
void ctrlZPress() override;
22+
void bPress() override;
23+
void vPress() override;
24+
void ctrlXPress();
25+
void modeFallen(ros::Duration duration);
26+
void modeNormalize();
27+
rm_common::BalanceCommandSender* balance_chassis_cmd_sender_{};
28+
29+
private:
30+
void balanceStateCallback(const rm_msgs::BalanceState::ConstPtr& msg);
31+
32+
ros::Subscriber state_sub_;
33+
double balance_dangerous_angle_;
34+
35+
InputEvent ctrl_x_event_, auto_fallen_event_;
36+
};
37+
} // namespace rm_manual

‎src/balance_manual.cpp‎

Lines changed: 7 additions & 78 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
//
2-
// Created by yuchen on 2023/4/3.
2+
// Created by kook on 9/28/24.
33
//
44

55
#include "rm_manual/balance_manual.h"
@@ -10,21 +10,9 @@ BalanceManual::BalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee)
1010
: ChassisGimbalShooterCoverManual(nh, nh_referee)
1111
{
1212
ros::NodeHandle balance_nh(nh, "balance");
13-
balance_cmd_sender_ = new rm_common::BalanceCommandSender(balance_nh);
14-
balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL);
15-
16-
nh.param("flank_frame", flank_frame_, std::string("flank_frame"));
17-
nh.param("reverse_frame", reverse_frame_, std::string("yaw_reverse_frame"));
18-
nh.param("balance_dangerous_angle", balance_dangerous_angle_, 0.3);
19-
13+
balance_nh.param("flank_frame", flank_frame_, std::string("flank_frame"));
14+
balance_nh.param("reverse_frame", reverse_frame_, std::string("yaw_reverse_frame"));
2015
is_balance_ = true;
21-
state_sub_ = balance_nh.subscribe<rm_msgs::BalanceState>("/state", 1, &BalanceManual::balanceStateCallback, this);
22-
x_event_.setRising(boost::bind(&BalanceManual::xPress, this));
23-
g_event_.setRising(boost::bind(&BalanceManual::gPress, this));
24-
v_event_.setRising(boost::bind(&BalanceManual::vPress, this));
25-
auto_fallen_event_.setActiveHigh(boost::bind(&BalanceManual::modeFallen, this, _1));
26-
auto_fallen_event_.setDelayTriggered(boost::bind(&BalanceManual::modeNormalize, this), 1.5, true);
27-
ctrl_x_event_.setRising(boost::bind(&BalanceManual::ctrlXPress, this));
2816
}
2917

3018
void BalanceManual::sendCommand(const ros::Time& time)
@@ -49,14 +37,11 @@ void BalanceManual::sendCommand(const ros::Time& time)
4937

5038
ChassisGimbalShooterManual::sendCommand(time);
5139
cover_command_sender_->sendCommand(time);
52-
balance_cmd_sender_->sendCommand(time);
5340
}
5441

5542
void BalanceManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data)
5643
{
5744
ChassisGimbalShooterCoverManual::checkKeyboard(dbus_data);
58-
v_event_.update(dbus_data->key_v && !dbus_data->key_ctrl);
59-
ctrl_x_event_.update(dbus_data->key_ctrl && dbus_data->key_x);
6045
}
6146

6247
void BalanceManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data)
@@ -72,15 +57,13 @@ void BalanceManual::rightSwitchDownRise()
7257
{
7358
ChassisGimbalShooterCoverManual::rightSwitchDownRise();
7459
state_ = RC;
75-
balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN);
7660
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FALLEN);
7761
chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
7862
}
7963

8064
void BalanceManual::rightSwitchMidRise()
8165
{
8266
ChassisGimbalShooterCoverManual::rightSwitchMidRise();
83-
balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL);
8467
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
8568
chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
8669
}
@@ -90,13 +73,11 @@ void BalanceManual::ctrlZPress()
9073
ChassisGimbalShooterCoverManual::ctrlZPress();
9174
if (supply_)
9275
{
93-
balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN);
9476
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FALLEN);
9577
chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
9678
}
9779
else
9880
{
99-
balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL);
10081
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
10182
chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
10283
}
@@ -111,18 +92,7 @@ void BalanceManual::shiftPress()
11192
{
11293
ChassisGimbalShooterCoverManual::shiftPress();
11394
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::UP_SLOPE);
114-
chassis_cmd_sender_->updateSafetyPower(60);
115-
}
116-
117-
void BalanceManual::vPress()
118-
{
119-
chassis_cmd_sender_->updateSafetyPower(80);
120-
}
121-
122-
void BalanceManual::bPress()
123-
{
124-
ChassisGimbalShooterCoverManual::bPress();
125-
chassis_cmd_sender_->updateSafetyPower(100);
95+
chassis_cmd_sender_->updateSafetyPower(220);
12696
}
12797

12898
void BalanceManual::wPress()
@@ -197,54 +167,13 @@ void BalanceManual::cPress()
197167
{
198168
if (is_gyro_)
199169
{
200-
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
201-
vel_cmd_sender_->setAngularZVel(0.0);
202-
is_gyro_ = false;
170+
setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
203171
}
204172
else
205173
{
206-
chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW);
207-
is_gyro_ = true;
208-
if (x_scale_ != 0.0 || y_scale_ != 0.0)
209-
vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
210-
else
211-
vel_cmd_sender_->setAngularZVel(1.0);
174+
setChassisMode(rm_msgs::ChassisCmd::RAW);
175+
chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
212176
}
213177
}
214178

215-
void BalanceManual::ctrlXPress()
216-
{
217-
if (balance_cmd_sender_->getMsg()->data == rm_msgs::BalanceState::NORMAL)
218-
balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN);
219-
else
220-
balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL);
221-
}
222-
223-
void BalanceManual::balanceStateCallback(const rm_msgs::BalanceState::ConstPtr& msg)
224-
{
225-
if ((ros::Time::now() - msg->header.stamp).toSec() < 0.2)
226-
{
227-
if (std::abs(msg->theta) > balance_dangerous_angle_)
228-
chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
229-
if (msg->mode == rm_msgs::BalanceState::NORMAL)
230-
auto_fallen_event_.update(std::abs(msg->theta) > 0.42 && std::abs(msg->x_dot) > 1.5 &&
231-
vel_cmd_sender_->getMsg()->linear.x == 0 && vel_cmd_sender_->getMsg()->linear.y == 0 &&
232-
vel_cmd_sender_->getMsg()->angular.z == 0);
233-
}
234-
}
235-
236-
void BalanceManual::modeNormalize()
237-
{
238-
balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL);
239-
ROS_INFO("mode normalize");
240-
}
241-
242-
void BalanceManual::modeFallen(ros::Duration duration)
243-
{
244-
if (duration.toSec() > 0.3)
245-
{
246-
balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN);
247-
ROS_INFO("mode fallen");
248-
}
249-
}
250179
} // namespace rm_manual

0 commit comments

Comments
 (0)