Skip to content

Add elevator_action_plugin #31

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
jamendezib wants to merge 1 commit into
base: ros-devel
Choose a base branch
from
Open

Add elevator_action_plugin #31

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion robotnik_pad_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,14 @@ find_package(catkin REQUIRED COMPONENTS
pluginlib
robotnik_pad
robotnik_pad_msgs
actionlib
)


catkin_package(
INCLUDE_DIRS include
LIBRARIES robotnik_pad_plugins
CATKIN_DEPENDS roscpp pluginlib robotnik_pad robotnik_pad_msgs
CATKIN_DEPENDS roscpp pluginlib robotnik_pad robotnik_pad_msgs actionlib
# DEPENDS system_lib
)

Expand All @@ -36,6 +37,7 @@ add_library(robotnik_pad_plugins
src/poi_plugin.cpp
src/ptz_plugin.cpp
src/blkarc_plugin.cpp
src/elevator_action_plugin.cpp
)

add_dependencies(robotnik_pad_plugins ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
Expand Down
38 changes: 38 additions & 0 deletions robotnik_pad_plugins/include/robotnik_pad_plugins/elevator_action_plugin.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
#ifndef PAD_PLUGIN_ELEVATOR_ACTION_H_
#define PAD_PLUGIN_ELEVATOR_ACTION_H_

#include <robotnik_msgs/ElevatorAction.h>
#include <robotnik_msgs/SetElevatorAction.h>
#include <robotnik_msgs/SetElevatorActionGoal.h>
#include <robotnik_msgs/SetElevatorGoal.h>
#include <robotnik_msgs/SetElevatorResult.h>
#include <robotnik_pad/generic_pad_plugin.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/simple_client_goal_state.h>

namespace pad_plugins
{
class PadPluginElevatorAction : public GenericPadPlugin
{
public:
PadPluginElevatorAction();
~PadPluginElevatorAction();

virtual void initialize(const ros::NodeHandle &nh, const std::string &plugin_ns);
virtual void execute(const std::vector<Button> &buttons, std::vector<float> &axes);

protected:
int button_dead_man_;
double axis_elevator_;
bool elevator_is_running_;
bool stop_elevator_;
bool stop_elevator_dead_man_;

std::string elevator_action_ns_;
std::shared_ptr<actionlib::SimpleActionClient<robotnik_msgs::SetElevatorAction>> elevator_action_client_;

bool sendGoal(const int action);
void actionDoneCb(const actionlib::SimpleClientGoalState& state, const robotnik_msgs::SetElevatorResultConstPtr& result);
};
}; // namespace pad_plugins
#endif // PAD_PLUGIN_ELEVATOR_ACTION_H_
2 changes: 2 additions & 0 deletions robotnik_pad_plugins/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
<build_depend>pluginlib</build_depend>
<build_depend>robotnik_pad</build_depend>
<build_depend>robotnik_pad_msgs</build_depend>
<build_depend>actionlib</build_depend>
Copy link

Copilot AI Nov 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Missing exec_depend for actionlib. Since actionlib is required at runtime for the action client, it should be added as an execution dependency similar to other dependencies like roscpp, pluginlib, etc.

Copilot uses AI. Check for mistakes.

<build_export_depend>roscpp</build_export_depend>
<exec_depend>roscpp</exec_depend>
Expand All @@ -26,6 +27,7 @@
<exec_depend>robotnik_pad_msgs</exec_depend>



<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
Expand Down
3 changes: 3 additions & 0 deletions robotnik_pad_plugins/robotnik_pad_pluginlib.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,4 +20,7 @@
<class name="robotnik_pad_plugins/BlkArc" type="pad_plugins::PadPluginBlkArc" base_class_type="pad_plugins::GenericPadPlugin">
<description>This is the BLK ARC plugin.</description>
</class>
<class name="robotnik_pad_plugins/ElevatorAction" type="pad_plugins::PadPluginElevatorAction" base_class_type="pad_plugins::GenericPadPlugin">
<description>This is the Pad elevator action plugin.</description>
</class>
</library>
92 changes: 92 additions & 0 deletions robotnik_pad_plugins/src/elevator_action_plugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,92 @@
#include <robotnik_pad_plugins/elevator_action_plugin.h>

namespace pad_plugins
{
PadPluginElevatorAction::PadPluginElevatorAction()
{
}

PadPluginElevatorAction::~PadPluginElevatorAction()
{
}

void PadPluginElevatorAction::initialize(const ros::NodeHandle& nh, const std::string& plugin_ns)
{
bool required = true;
bool not_required = false;

pnh_ = ros::NodeHandle(nh, plugin_ns);

readParam(pnh_, "config/deadman", button_dead_man_, button_dead_man_, required);
readParam(pnh_, "config/axis_elevator", axis_elevator_, axis_elevator_, required);
readParam(pnh_, "elevator_action_ns", elevator_action_ns_, elevator_action_ns_, required);
readParam(pnh_, "stop_elevator", stop_elevator_, false, not_required);
readParam(pnh_, "stop_elevator_dead_man", stop_elevator_dead_man_, false, not_required);
// Service client
Copy link

Copilot AI Nov 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Incorrect comment: This is an action client, not a service client. The comment should be updated to reflect that this creates an actionlib action client.

Suggested change
// Service client
// Actionlib action client

Copilot uses AI. Check for mistakes.
elevator_action_client_ = std::make_shared
<actionlib::SimpleActionClient<robotnik_msgs::SetElevatorAction>>(nh_, elevator_action_ns_, true);
Comment on lines +26 to +27
Copy link

Copilot AI Nov 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The action client is created using nh_ from the base class, but unlike other plugins (e.g., poi_plugin.cpp, blkarc_plugin.cpp), nh_ is not initialized in this plugin's initialize() method. This will cause the action client to be created with an uninitialized node handle. Add nh_ = ros::NodeHandle(); before line 26, similar to poi_plugin.cpp line 20.

Copilot uses AI. Check for mistakes.

elevator_is_running_ = false;
}

void PadPluginElevatorAction::execute(const std::vector<Button>& buttons, std::vector<float>& axes)
{
if (buttons[button_dead_man_].isPressed())
{
if (axes[axis_elevator_] > 0.95)
{
if (!elevator_is_running_)
{
elevator_is_running_ = sendGoal(robotnik_msgs::ElevatorAction::RAISE);
}
}
if (axes[axis_elevator_] < -0.95)
{
if (!elevator_is_running_)
{
elevator_is_running_ = sendGoal(robotnik_msgs::ElevatorAction::LOWER);
}
}

if (stop_elevator_)
{
if (axes[axis_elevator_] > -0.1 && axes[axis_elevator_] < 0.1 && elevator_is_running_ == true)
Copy link

Copilot AI Nov 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[nitpick] Redundant comparison: The expression elevator_is_running_ == true can be simplified to elevator_is_running_ for better readability and consistency with line 38 and line 45 which use !elevator_is_running_.

Suggested change
if (axes[axis_elevator_] > -0.1 && axes[axis_elevator_] < 0.1 && elevator_is_running_ == true)
if (axes[axis_elevator_] > -0.1 && axes[axis_elevator_] < 0.1 && elevator_is_running_)

Copilot uses AI. Check for mistakes.
{
elevator_is_running_ = !sendGoal(robotnik_msgs::ElevatorAction::STOP);
Copy link

Copilot AI Nov 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Inconsistent state management: When stopping the elevator (line 55), elevator_is_running_ is set to the negation of sendGoal() result. If sendGoal() returns true (success), this sets elevator_is_running_ = false, which is correct. However, if sendGoal() returns false (failure), this sets elevator_is_running_ = true, incorrectly indicating the elevator is still running even though the STOP command failed. The same issue exists on line 64. Consider setting elevator_is_running_ = false unconditionally after attempting to stop, or handle the failure case explicitly.

Copilot uses AI. Check for mistakes.
}
}

}
else if (buttons[button_dead_man_].isReleased())
{
if (stop_elevator_dead_man_ && elevator_is_running_)
{
elevator_is_running_ = !sendGoal(robotnik_msgs::ElevatorAction::STOP);
Copy link

Copilot AI Nov 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Same inconsistent state management issue as line 55: If sendGoal() fails and returns false, this will set elevator_is_running_ = true, incorrectly indicating the elevator is still running. Consider setting elevator_is_running_ = false unconditionally or handling the failure case explicitly.

Copilot uses AI. Check for mistakes.
}
}
}

void PadPluginElevatorAction::actionDoneCb(const actionlib::SimpleClientGoalState& state, const robotnik_msgs::SetElevatorResultConstPtr& result)
{
elevator_is_running_ = false;
}

bool PadPluginElevatorAction::sendGoal(const int action)
{
robotnik_msgs::SetElevatorGoal elevator_goal;
elevator_goal.action.action = action;
ROS_INFO_NAMED("PadPluginElevatorAction", "PadPluginElevatorAction::execute: %d", action);
Comment on lines +74 to +78
Copy link

Copilot AI Nov 25, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Error message is unclear: The log message "PadPluginElevatorAction::execute: %d" doesn't clearly indicate what action is being performed. Consider using more descriptive messages like "Sending elevator action: %d" or adding action name strings (RAISE/LOWER/STOP) to make debugging easier.

Suggested change
bool PadPluginElevatorAction::sendGoal(const int action)
{
robotnik_msgs::SetElevatorGoal elevator_goal;
elevator_goal.action.action = action;
ROS_INFO_NAMED("PadPluginElevatorAction", "PadPluginElevatorAction::execute: %d", action);
// Helper function to convert action int to string
static const char* elevatorActionToString(int action) {
switch (action) {
case robotnik_msgs::ElevatorAction::RAISE:
return "RAISE";
case robotnik_msgs::ElevatorAction::LOWER:
return "LOWER";
case robotnik_msgs::ElevatorAction::STOP:
return "STOP";
default:
return "UNKNOWN";
}
}
bool PadPluginElevatorAction::sendGoal(const int action)
{
robotnik_msgs::SetElevatorGoal elevator_goal;
elevator_goal.action.action = action;
ROS_INFO_NAMED("PadPluginElevatorAction", "Sending elevator action: %s (%d)", elevatorActionToString(action), action);

Copilot uses AI. Check for mistakes.
elevator_action_client_->sendGoal(elevator_goal,
boost::bind(&PadPluginElevatorAction::actionDoneCb, this, _1, _2));
auto state = elevator_action_client_->getState();
if (!(state == actionlib::SimpleClientGoalState::PENDING ||
state == actionlib::SimpleClientGoalState::ACTIVE))
{
ROS_ERROR_NAMED("PadPluginElevatorAction", "PadPluginElevatorAction::execute: Goal was not accepted. Current state: %s", state.toString().c_str());
return false;
}
return true;
}


} // namespace pad_plugins
2 changes: 2 additions & 0 deletions robotnik_pad_plugins/src/generic_pad_plugins.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include <robotnik_pad_plugins/poi_plugin.h>
#include <robotnik_pad_plugins/ptz_plugin.h>
#include <robotnik_pad_plugins/blkarc_plugin.h>
#include <robotnik_pad_plugins/elevator_action_plugin.h>

PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginMovement, pad_plugins::GenericPadPlugin);
PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginSafetyMovement, pad_plugins::GenericPadPlugin);
Expand All @@ -16,3 +17,4 @@ PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginAckermannMovement, pad_plugins::Gen
PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginPoi, pad_plugins::GenericPadPlugin);
PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginPtz, pad_plugins::GenericPadPlugin);
PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginBlkArc, pad_plugins::GenericPadPlugin);
PLUGINLIB_EXPORT_CLASS(pad_plugins::PadPluginElevatorAction, pad_plugins::GenericPadPlugin);