Skip to content

Commit

Permalink
Adding flippers
Browse files Browse the repository at this point in the history
  • Loading branch information
@IliTheButterfly
IliTheButterfly committed Oct 7, 2024
1 parent d589799 commit 1096906
Show file tree
Hide file tree
Showing 3 changed files with 139 additions and 8 deletions.
89 changes: 82 additions & 7 deletions src/rove_bringup/src/rove_controller_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,19 @@ using GripperCommand = control_msgs::action::GripperCommand;
using GoalHandleGripperCommand = rclcpp_action::ClientGoalHandle<GripperCommand>;

enum ControllerMode{
FLIPPER_MODE = 0,
ARM_MODE = 1,
TRACKS_MODE = 0,
FLIPPER_MODE,
ARM_MODE,
CONTROLLER_MODE_COUNT,
};

enum FlipperMode{
NO_FLIPPERS = 0b000,
FRONT_FLIPPERS = 0b010,
REAR_FLIPPERS = 0b001,
ALL_FLIPPERS = 0b011,
INDEPENDANT_FLAG = 0b100,
};

class RoveController : public rclcpp::Node {
public:
Expand Down Expand Up @@ -50,6 +59,7 @@ class RoveController : public rclcpp::Node {
// cmd_vel_sub_ = create_subscription<geometry_msgs::msg::Twist>("/cmd_vel", 1, std::bind(&RoveController::cmdvelCallback, this, std::placeholders::_1));

joy_pub_ = create_publisher<sensor_msgs::msg::Joy>("/rove/joy", 1);
flipper_pub_ = create_publisher<std_msgs::msg::Float64MultiArray>("/rove/commands", 0);

// auto x = create_wall_timer(
// std::chrono::milliseconds(1000/20), std::bind(&RoveController::cmdvelTimerCallback, this, std::placeholders::_1)
Expand Down Expand Up @@ -77,12 +87,14 @@ class RoveController : public rclcpp::Node {
private:
int A, B, X, Y, LB, RB, VIEW, MENU, XBOX, LS, RS, SHARE, LS_X, LS_Y, LT, RS_X, RS_Y, RT, D_PAD_X, D_PAD_Y;

int selected_mode = FLIPPER_MODE;
int selected_mode = TRACKS_MODE;
int flipper_mode = ALL_FLIPPERS;
int flipper_y = 0;

rclcpp_action::Client<GripperCommand>::SharedPtr gripper_action_client_;

void nextMode(){
selected_mode = (selected_mode + 1) % 2; // Number of modes
selected_mode = (selected_mode + 1) % CONTROLLER_MODE_COUNT; // Number of modes
RCLCPP_INFO(get_logger(), "profil %i", selected_mode);
}

Expand Down Expand Up @@ -123,14 +135,69 @@ class RoveController : public rclcpp::Node {

}

void flipper_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) {
log("Flipper...");
void tracks_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) {
// log("Flipper...");
teleop_msg_.axes[0] = joy_msg->axes[LS_X];
teleop_msg_.axes[1] = joy_msg->axes[LS_Y];
teleop_msg_.buttons[0] = joy_msg->buttons[A];
joy_pub_->publish(teleop_msg_);
}

void flipper_action(const sensor_msgs::msg::Joy::SharedPtr joy_msg) {
flipper_y = std::max(-2, std::min(2, flipper_y - dpad_down(joy_msg, D_PAD_Y, -1) + dpad_down(joy_msg, D_PAD_Y, 1)));

if (buttton_down(joy_msg, X))
{
flipper_y = 0;
}
auto msg = std_msgs::msg::Float64MultiArray();
msg.data.resize(4, 0);

flipper_mode = 0;

if (flipper_y == 0) flipper_mode = ALL_FLIPPERS;
if (abs(flipper_y) == 2) flipper_mode |= INDEPENDANT_FLAG;
if (flipper_mode > 0) flipper_mode |= FRONT_FLIPPERS;
if (flipper_mode < 0) flipper_mode |= REAR_FLIPPERS;

switch (flipper_mode)
{
case ALL_FLIPPERS:
msg.data[0] = joy_msg->axes[LS_Y];
msg.data[1] = joy_msg->axes[LS_Y];
msg.data[2] = joy_msg->axes[LS_Y];
msg.data[3] = joy_msg->axes[LS_Y];
break;
case FRONT_FLIPPERS:
msg.data[0] = joy_msg->axes[LS_Y];
msg.data[1] = joy_msg->axes[LS_Y];
msg.data[2] = 0;
msg.data[3] = 0;
break;
case FRONT_FLIPPERS | INDEPENDANT_FLAG:
msg.data[0] = joy_msg->axes[LS_Y];
msg.data[1] = joy_msg->axes[RS_Y];
msg.data[2] = 0;
msg.data[3] = 0;
break;
case REAR_FLIPPERS:
msg.data[0] = 0;
msg.data[1] = 0;
msg.data[2] = joy_msg->axes[LS_Y];
msg.data[3] = joy_msg->axes[LS_Y];
break;
case REAR_FLIPPERS | INDEPENDANT_FLAG:
msg.data[0] = 0;
msg.data[1] = 0;
msg.data[2] = joy_msg->axes[LS_Y];
msg.data[3] = joy_msg->axes[RS_Y];
break;

default:
break;
}
}

bool buttton_down(sensor_msgs::msg::Joy::SharedPtr curr_msg, int index){
return previous_msg_.buttons[index] != curr_msg->buttons[index] && curr_msg->buttons[index] == 1;
}
Expand All @@ -141,6 +208,13 @@ class RoveController : public rclcpp::Node {
return previous_msg_.buttons[index] == curr_msg->buttons[index] && curr_msg->buttons[index] == 1;
}

bool dpad_down(sensor_msgs::msg::Joy::SharedPtr curr_msg, int index, int value){
return previous_msg_.axes[index] != curr_msg->axes[index] && curr_msg->axes[index] == value;
}
bool dpad_up(sensor_msgs::msg::Joy::SharedPtr curr_msg, int index, int value){
return previous_msg_.axes[index] != curr_msg->axes[index] && curr_msg->axes[index] != value;
}

void log(const char *text){
static rclcpp::Clock clk{};
static const char* last = nullptr;
Expand All @@ -157,7 +231,7 @@ class RoveController : public rclcpp::Node {
arm_action(joy_msg);
break;
case FLIPPER_MODE:
flipper_action(joy_msg);
tracks_action(joy_msg);
break;
}

Expand All @@ -177,6 +251,7 @@ class RoveController : public rclcpp::Node {
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
// rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
rclcpp::Publisher<sensor_msgs::msg::Joy>::SharedPtr joy_pub_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr flipper_pub_;
sensor_msgs::msg::Joy previous_msg_;
sensor_msgs::msg::Joy teleop_msg_;
// geometry_msgs::msg::Twist cmd_vel_msg_;
Expand Down
11 changes: 11 additions & 0 deletions src/rove_description/config/tracks_controllers.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,12 +17,23 @@ controller_manager:
# forward_controller:
# type: forward_command_controller/ForwardCommandController

flipper_controller:
type: position_controllers/JointGroupPositionController

# forward_controller:
# ros__parameters:
# joints:
# - track_fl_j
# interface_name: velocity

flipper_controller:
ros__parameters:
joints:
- flipper_rr_j
- flipper_fr_j
- flipper_fl_j
- flipper_rl_j

diff_drive_controller:
ros__parameters:
# left_wheel_names: ["track_l_j"]
Expand Down
47 changes: 46 additions & 1 deletion src/rove_description/urdf/rove.ros2_control.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,29 @@
</joint>
</xacro:macro>

<xacro:macro name="flipper_motor_control" params="suffix nodeid">
<joint name="flipper_${suffix}_j">
<param name="node_id">${nodeid}</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
<state_interface name="voltage"/>
<state_interface name="current"/>
</joint>
</xacro:macro>

<xacro:macro name="flipper_fake_motor_control" params="suffix">
<joint name="flipper_${suffix}_j">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
<state_interface name="voltage"/>
<state_interface name="current"/>
</joint>
</xacro:macro>

<xacro:macro name="rove_odrive_motor" params="suffix id">
<xacro:unless value="${(id == -1)}">
<xacro:track_motor_control suffix="${suffix}" nodeid="${id}"/>
Expand All @@ -35,7 +58,19 @@
</xacro:if>
</xacro:macro>

<xacro:macro name="odrive_control" params="fl_id:=21 rl_id:=22 fr_id:=23 rr_id:=24">
<xacro:macro name="rove_odrive_flipper" params="suffix id">
<xacro:unless value="${(id == -1)}">
<xacro:flipper_motor_control suffix="${suffix}" nodeid="${id}"/>
</xacro:unless>
</xacro:macro>

<xacro:macro name="rove_fake_odrive_flipper" params="suffix id">
<xacro:if value="${(id == -1)}">
<xacro:flipper_fake_motor_control suffix="${suffix}"/>
</xacro:if>
</xacro:macro>

<xacro:macro name="odrive_control" params="fl_id:=21 rl_id:=22 fr_id:=23 rr_id:=24 ffl_id:=11 frl_id:=12 ffr_id:=13 frr_id:=14">
<ros2_control name="odrive_control" type="system">
<hardware>
<plugin>odrive_ros2_control_plugin/ODriveHardwareInterface</plugin>
Expand All @@ -45,6 +80,11 @@
<xacro:rove_odrive_motor suffix="rl" id="${rl_id}"/>
<xacro:rove_odrive_motor suffix="fr" id="${fr_id}"/>
<xacro:rove_odrive_motor suffix="rr" id="${rr_id}"/>

<xacro:rove_odrive_motor suffix="fl" id="${ffl_id}"/>
<xacro:rove_odrive_motor suffix="rl" id="${frl_id}"/>
<xacro:rove_odrive_motor suffix="fr" id="${ffr_id}"/>
<xacro:rove_odrive_motor suffix="rr" id="${frr_id}"/>
</ros2_control>

<ros2_control name="fake_odrive_control" type="system">
Expand All @@ -55,6 +95,11 @@
<xacro:rove_fake_odrive_motor suffix="rl" id="${rl_id}"/>
<xacro:rove_fake_odrive_motor suffix="fr" id="${fr_id}"/>
<xacro:rove_fake_odrive_motor suffix="rr" id="${rr_id}"/>

<xacro:rove_fake_odrive_flipper suffix="fl" id="${ffl_id}"/>
<xacro:rove_fake_odrive_flipper suffix="rl" id="${frl_id}"/>
<xacro:rove_fake_odrive_flipper suffix="fr" id="${ffr_id}"/>
<xacro:rove_fake_odrive_flipper suffix="rr" id="${frr_id}"/>
</ros2_control>
</xacro:macro>

Expand Down

0 comments on commit 1096906

Please sign in to comment.