From 43c5d12529669971852ac4a165189043971995f4 Mon Sep 17 00:00:00 2001 From: aswathselvam Date: Mon, 13 Dec 2021 10:16:45 -0500 Subject: [PATCH] 3.04 Integrate ActionLib Integrate action lib in inversekinematics class --- CMakeLists.txt | 16 ++++++++++++++- action/swarmACT.action | 2 +- include/swarm_robots/agent_node.hpp | 9 +++++++++ include/swarm_robots/inverse_kinematics.hpp | 8 ++++++++ src/action_client.cpp | 19 ++++++++++++++++++ src/action_server.cpp | 22 +++++++++++++++++++++ src/agent_node.cpp | 6 ++++-- src/inverse_kinematics.cpp | 22 ++++++++++++++++++++- 8 files changed, 99 insertions(+), 5 deletions(-) create mode 100644 src/action_client.cpp create mode 100644 src/action_server.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6d52965..333c574 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,7 +36,8 @@ catkin_package( # INCLUDE_DIRS include # LIBRARIES swarm_robots # CATKIN_DEPENDS roscpp std_msgs -# DEPENDS system_lib + DEPENDS + actionlib actionlib_msgs ) @@ -73,6 +74,19 @@ src/arena.cpp ) + add_executable(${PROJECT_NAME}_action_client_node + src/action_client.cpp + ) +target_link_libraries(${PROJECT_NAME}_action_client_node ${catkin_LIBRARIES}) + + + add_executable(${PROJECT_NAME}_action_server_node + src/action_server.cpp + ) +target_link_libraries(${PROJECT_NAME}_action_server_node ${catkin_LIBRARIES}) + + + ############# ## Install ## ############# diff --git a/action/swarmACT.action b/action/swarmACT.action index 2e97c4e..fd37896 100644 --- a/action/swarmACT.action +++ b/action/swarmACT.action @@ -1,5 +1,5 @@ # Define the goal -uint32 dishwasher_id # Specify which dishwasher we want to use +uint32 dishwasher_id =1 # Specify which dishwasher we want to use --- # Define the result uint32 total_dishes_cleaned diff --git a/include/swarm_robots/agent_node.hpp b/include/swarm_robots/agent_node.hpp index 45060cf..2aadcd8 100644 --- a/include/swarm_robots/agent_node.hpp +++ b/include/swarm_robots/agent_node.hpp @@ -78,6 +78,15 @@ class AgentNode : public Agent { bool SwarmService(swarm_robots::service::Request &req, // NOLINT swarm_robots::service::Response &res); // NOLINT + /** + * @brief Service function that is called with /service, this function changes + * the message according to the user's input. + * @param req : Request object reference from service file, it contains the + * parameters related to the service input. + * @param res : Response object reference from service file, it contains the + * parameteres of the service output. + */ + private: // NOLINT int krate_; ///< Variable to store ros loop rate ros::NodeHandlePtr nh_; ///< Variable to store ros node handle for executing node // NOLINT diff --git a/include/swarm_robots/inverse_kinematics.hpp b/include/swarm_robots/inverse_kinematics.hpp index d9e02dc..d7e3eab 100644 --- a/include/swarm_robots/inverse_kinematics.hpp +++ b/include/swarm_robots/inverse_kinematics.hpp @@ -15,13 +15,17 @@ #include #include #include +#include +#include // Note: "Action" is appended #include + #include "safety_check.hpp" // NOLINT #include "state.hpp" // NOLINT using std::string; +typedef actionlib::SimpleActionServer Server; class InverseKinematics { public: // NOLINT @@ -52,11 +56,15 @@ class InverseKinematics { * @return State: Output x,y,x coordinates */ State CheckSafety(); + void execute(const swarm_robots::swarmACTGoalConstPtr& goal, Server* as); + private: // NOLINT State goal_location_; ///< Goal x,y,z coordinares State current_location_; ///< Current x,y,z coordinares State velocity_; ///< Variable to store x,y,z velocities SafetyCheck* safety_check_; ///< reference to Safety_check class + Server server_; + }; #endif // INCLUDE_SWARM_ROBOTS_INVERSE_KINEMATICS_HPP_ // NOLINT diff --git a/src/action_client.cpp b/src/action_client.cpp new file mode 100644 index 0000000..f69642f --- /dev/null +++ b/src/action_client.cpp @@ -0,0 +1,19 @@ +#include // Note: "Action" is appended +#include + +typedef actionlib::SimpleActionClient Client; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "do_dishes_client"); + Client client("do_dishes", true); // true -> don't need ros::spin() + client.waitForServer(); + swarm_robots::swarmACTGoal ping; + // Fill in goal here + client.sendGoal(ping); + client.waitForResult(ros::Duration(5.0)); + if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + printf("Yay! The dishes are now clean"); + printf("Current State: %s\n", client.getState().toString().c_str()); + return 0; +} \ No newline at end of file diff --git a/src/action_server.cpp b/src/action_server.cpp new file mode 100644 index 0000000..272756e --- /dev/null +++ b/src/action_server.cpp @@ -0,0 +1,22 @@ +#include // Note: "Action" is appended +#include +#include + +typedef actionlib::SimpleActionServer Server; + +void execute(const swarm_robots::swarmACTGoalConstPtr& goal, Server* as) +{ + // Do lots of awesome groundbreaking robot stuff here + std::cout<setSucceeded(); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "do_dishes_server"); + ros::NodeHandle n; + Server server(n, "do_dishes", boost::bind(&execute, _1, &server), false); + server.start(); + ros::spin(); + return 0; +} \ No newline at end of file diff --git a/src/agent_node.cpp b/src/agent_node.cpp index 0ccbd8b..007ff88 100644 --- a/src/agent_node.cpp +++ b/src/agent_node.cpp @@ -18,6 +18,7 @@ #include #include + #include #include @@ -27,6 +28,8 @@ using std::string; +typedef actionlib::SimpleActionServer Server; + AgentNode::AgentNode(std::string ns): Agent::Agent(ns){ this->agent_id = ns; @@ -42,8 +45,6 @@ AgentNode::AgentNode(std::string ns): Agent::Agent(ns){ this->pos_sub_ = this->nh_->subscribe("/jackal" + ns + "/base_pose_ground_truth", 1, &AgentNode::PosCallback, this); - this->service_ = nh_->advertiseService("service", &AgentNode::SwarmService, this); - this->krate_ = 20; } @@ -112,6 +113,7 @@ bool AgentNode::SwarmService(swarm_robots::service::Request &req, // NOLINT } } + AgentNode::~AgentNode() { std::cout << "Deteletd NH"; } diff --git a/src/inverse_kinematics.cpp b/src/inverse_kinematics.cpp index a3c651f..a5fc392 100644 --- a/src/inverse_kinematics.cpp +++ b/src/inverse_kinematics.cpp @@ -10,6 +10,8 @@ */ #include "swarm_robots/inverse_kinematics.hpp" +#include +#include #include #include @@ -17,12 +19,30 @@ #include "swarm_robots/state.hpp" -InverseKinematics::InverseKinematics(string ns, ros::NodeHandlePtr nh) { +using std::string; +typedef actionlib::SimpleActionServer Server; + + + +InverseKinematics::InverseKinematics(string ns, ros::NodeHandlePtr nh): + server_(*nh, "jackal"+ns, boost::bind(&InverseKinematics::execute, this, _1), false) + { // safety_check_ = new SafetyCheck(ns, nh); // CPP CHECK Error SafetyCheck safety_check(ns, nh); safety_check_ = &safety_check; + server_.start(); + } + +void InverseKinematics::execute(const swarm_robots::swarmACTGoalConstPtr& goal, Server* as) +{ + // Do lots of awesome groundbreaking robot stuff here + std::cout<setSucceeded(); +} + + State InverseKinematics::PerformIK(State start, State goal) { this->current_location_ = start; this->goal_location_ = goal;