From d3bff7a45e8cae7c060f10f744a0b229190c45af Mon Sep 17 00:00:00 2001
From: Irene Garcia Camacho <igarcia@iri.upc.edu>
Date: Mon, 29 Jul 2019 13:55:58 +0000
Subject: [PATCH] Added function to play motion module to use new loaded motion
by its name
---
include/tiago_modules/play_motion_module.h | 1 +
src/play_motion_module.cpp | 21 +++++++++++++++++++++
2 files changed, 22 insertions(+)
diff --git a/include/tiago_modules/play_motion_module.h b/include/tiago_modules/play_motion_module.h
index f383d44..0ab39ae 100644
--- a/include/tiago_modules/play_motion_module.h
+++ b/include/tiago_modules/play_motion_module.h
@@ -61,6 +61,7 @@ class CPlayMotionModule : public CModule<play_motion_module::PlayMotionModuleCon
public:
CPlayMotionModule(const std::string &name,const std::string &name_space=std::string(""));
void execute_motion(play_motion_ids motion_id);
+ void execute_motion(std::string motion_name);
void stop(void);
bool is_finished(void);
play_motion_module_status_t get_status(void);
diff --git a/src/play_motion_module.cpp b/src/play_motion_module.cpp
index 003a96c..ef17644 100644
--- a/src/play_motion_module.cpp
+++ b/src/play_motion_module.cpp
@@ -165,6 +165,27 @@ void CPlayMotionModule::execute_motion(play_motion_ids motion_id)
}
}
+void CPlayMotionModule::execute_motion(std::string motion_name)
+{
+ std::string motion;
+
+ if(this->is_finished())
+ {
+ if(this->module_nh.getParam("/play_motion/motions/" + motion_name + "/meta/name", motion)) //If motion exists
+ {
+ this->goal.motion_name=motion_name;
+ this->goal.skip_planning=false;
+ this->goal.priority=0;
+ this->new_motion=true;
+ }
+ else
+ {
+ ROS_WARN("CPlayMotionModule: Motion name does not exist");
+ this->status=PLAY_MOTION_MODULE_INVALID_ID;
+ }
+ }
+}
+
void CPlayMotionModule::stop(void)
{
if(this->state!=PLAY_MOTION_MODULE_IDLE)
--
GitLab