diff --git a/.github/workflows/build_and_test_humble.yaml b/.github/workflows/build_and_test_humble.yaml index 6314c9d..f83601f 100644 --- a/.github/workflows/build_and_test_humble.yaml +++ b/.github/workflows/build_and_test_humble.yaml @@ -1,6 +1,6 @@ # This is a basic workflow to help you get started with Actions -name: Build and Test (humble) +name: Build (humble) # Controls when the action will run. on: @@ -32,11 +32,6 @@ jobs: use-ros2-testing: true - uses: ros-tooling/action-ros-ci@v0.3 with: + skip-tests: true target-ros2-distro: humble - colcon-defaults: | - { - "build": { - "symlink-install": false - } - } vcs-repo-file-url: dependencies.repos \ No newline at end of file diff --git a/.github/workflows/build_and_test_iron.yaml b/.github/workflows/build_and_test_iron.yaml index 9032011..27a2d2b 100644 --- a/.github/workflows/build_and_test_iron.yaml +++ b/.github/workflows/build_and_test_iron.yaml @@ -32,11 +32,6 @@ jobs: use-ros2-testing: true - uses: ros-tooling/action-ros-ci@v0.3 with: + skip-tests: true target-ros2-distro: iron - colcon-defaults: | - { - "build": { - "symlink-install": false - } - } vcs-repo-file-url: dependencies.repos \ No newline at end of file diff --git a/.github/workflows/build_and_test_rolling.yaml b/.github/workflows/build_and_test_rolling.yaml index 82bed30..051c1b9 100644 --- a/.github/workflows/build_and_test_rolling.yaml +++ b/.github/workflows/build_and_test_rolling.yaml @@ -1,6 +1,6 @@ # This is a basic workflow to help you get started with Actions -name: Build and Test (rolling) +name: Build (rolling) # Controls when the action will run. on: @@ -32,11 +32,6 @@ jobs: use-ros2-testing: true - uses: ros-tooling/action-ros-ci@master with: + skip-tests: true target-ros2-distro: rolling - colcon-defaults: | - { - "build": { - "symlink-install": false - } - } vcs-repo-file-url: dependencies.repos \ No newline at end of file diff --git a/README.md b/README.md index 1b4a396..102ca0a 100644 --- a/README.md +++ b/README.md @@ -24,8 +24,3 @@ you need only move the limbs of interest in the desired manner and the robot wil For more info, please refer to the related [paper](https://arxiv.org/abs/2403.13960). - - -### Note on CI - -The failing status is due to the colcon `--symlink-install` option that [action-ros-ci](https://github.com/ros-tooling/action-ros-ci) uses. At the moment the symlinking cannot be disabled but we are actively working with action-ros-ci maintainers (see issue [815](https://github.com/ros-tooling/action-ros-ci/issues/815)). \ No newline at end of file diff --git a/hni_cpp/CMakeLists.txt b/hni_cpp/CMakeLists.txt index 73047cb..ca72a49 100644 --- a/hni_cpp/CMakeLists.txt +++ b/hni_cpp/CMakeLists.txt @@ -420,11 +420,11 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights # comment the line when a copyright and license is added to all source files -set(ament_cmake_copyright_FOUND TRUE) +#set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) # comment the line when this package is in a git repo and when # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) + #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/hni_cpp/include/hni_cpp/chat_action_client.hpp b/hni_cpp/include/hni_cpp/chat_action_client.hpp index 674e669..e1aefa3 100644 --- a/hni_cpp/include/hni_cpp/chat_action_client.hpp +++ b/hni_cpp/include/hni_cpp/chat_action_client.hpp @@ -18,9 +18,9 @@ #include #include #include -#include // std::cout +#include // std::cout #include -#include // std::string, std::stof +#include // std::string, std::stof #include #include //#include @@ -37,31 +37,28 @@ #include "rclcpp_components/register_node_macro.hpp" #include "std_srvs/srv/set_bool.hpp" -namespace hni_chat_action_client { +namespace hni_chat_action_client +{ -class ChatActionClient : public rclcpp::Node { - public: - explicit ChatActionClient(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); - virtual ~ChatActionClient(); +class ChatActionClient : public rclcpp::Node +{ +public: + explicit ChatActionClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); + virtual ~ChatActionClient(); - private: +private: + void sendAsyncGoal(); + void + goalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); + void feedbackCallback(rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback); + void resultCallback(const rclcpp_action::ClientGoalHandle::WrappedResult& result); - void sendAsyncGoal(); - void goalResponseCallback( - const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle); - void feedbackCallback( - rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback); - void resultCallback( - const rclcpp_action::ClientGoalHandle::WrappedResult & result); + rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::TimerBase::SharedPtr timer_; +}; // ChatActionClient - rclcpp_action::Client::SharedPtr client_ptr_; - rclcpp::TimerBase::SharedPtr timer_; +} // namespace hni_chat_action_client - -}; // ChatActionClient - -} // hni_chat_action_client - -#endif // HNI_MOVES__CHAT_ACTION_CLIENT_HPP_ \ No newline at end of file +#endif // HNI_MOVES__CHAT_ACTION_CLIENT_HPP_ \ No newline at end of file diff --git a/hni_cpp/include/hni_cpp/chat_action_server.hpp b/hni_cpp/include/hni_cpp/chat_action_server.hpp index 5d04f49..5c7ce0d 100644 --- a/hni_cpp/include/hni_cpp/chat_action_server.hpp +++ b/hni_cpp/include/hni_cpp/chat_action_server.hpp @@ -18,9 +18,9 @@ #include #include #include -#include // std::cout +#include // std::cout #include -#include // std::string, std::stof +#include // std::string, std::stof #include #include //#include @@ -45,89 +45,80 @@ #include "rclcpp_components/register_node_macro.hpp" #include "std_srvs/srv/set_bool.hpp" - -namespace hni_chat_action_server { - - -class ChatActionServer : public rclcpp::Node { - public: - explicit ChatActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); - virtual ~ChatActionServer(); - - private: - - // services clients - rclcpp::Client::SharedPtr gstt_srv_client_; - rclcpp::Client::SharedPtr gtts_srv_client_; - rclcpp::Client::SharedPtr chat_srv_client_; - - //client classes - //std::shared_ptr led_srv_client_; - - // chat play action server - rclcpp_action::Server::SharedPtr action_server_; - std::shared_ptr> goal_handle_; - rclcpp_action::GoalResponse handleGoal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); - rclcpp_action::CancelResponse handleCancel( - const std::shared_ptr> goal_handle); - void handleAccepted( - const std::shared_ptr> goal_handle); - void execute( - const std::shared_ptr> goal_handle); - - - // joints play action client - rclcpp_action::Client::SharedPtr joints_act_client_; - void jointsPlayGoalResponseCallback( - const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle); - void jointsPlayFeedbackCallback( - rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback); - void jointsPlayResultCallback( - const rclcpp_action::ClientGoalHandle::WrappedResult & result); - - //leds play action client - rclcpp_action::Client::SharedPtr leds_play_act_client_; - - void ledsPlayGoalResponseCallback( - const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle); - void ledsPlayFeedbackCallback( - rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback); - void ledsPlayResultCallback( - const rclcpp_action::ClientGoalHandle::WrappedResult & result); - - - // parameters - const double kSecPerWord_ ; - const double kForwardParam_ ; - std::unordered_map moves_map_; - - /* - std::shared_ptr joints_play_client_; - std::shared_ptr gstt_srv_client_; - std::shared_ptr gtts_srv_client_; - std::shared_ptr chat_srv_client_; - */ - - rclcpp_action::ClientGoalHandle::SharedPtr head_goal_handle_; - rclcpp_action::ClientGoalHandle::SharedPtr eyes_goal_handle_; - rclcpp_action::ClientGoalHandle::SharedPtr ears_goal_handle_; - rclcpp_action::ClientGoalHandle::SharedPtr chest_goal_handle_; - - void eyesStatic(bool flag); - void earsStatic(bool flag); - void chestStatic(bool flag); - void headStatic(bool flag); - void headLoop(bool flag); - void earsLoop(bool flag); - void eyesLoop(bool flag); - - +namespace hni_chat_action_server +{ + +class ChatActionServer : public rclcpp::Node +{ +public: + explicit ChatActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); + virtual ~ChatActionServer(); + +private: + // services clients + rclcpp::Client::SharedPtr gstt_srv_client_; + rclcpp::Client::SharedPtr gtts_srv_client_; + rclcpp::Client::SharedPtr chat_srv_client_; + + // client classes + // std::shared_ptr led_srv_client_; + + // chat play action server + rclcpp_action::Server::SharedPtr action_server_; + std::shared_ptr> goal_handle_; + rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse + handleCancel(const std::shared_ptr> goal_handle); + void + handleAccepted(const std::shared_ptr> goal_handle); + void execute(const std::shared_ptr> goal_handle); + + // joints play action client + rclcpp_action::Client::SharedPtr joints_act_client_; + void jointsPlayGoalResponseCallback( + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); + void jointsPlayFeedbackCallback(rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback); + void jointsPlayResultCallback( + const rclcpp_action::ClientGoalHandle::WrappedResult& result); + + // leds play action client + rclcpp_action::Client::SharedPtr leds_play_act_client_; + + void ledsPlayGoalResponseCallback( + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); + void ledsPlayFeedbackCallback(rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback); + void ledsPlayResultCallback( + const rclcpp_action::ClientGoalHandle::WrappedResult& result); + + // parameters + const double kSecPerWord_; + const double kForwardParam_; + std::unordered_map moves_map_; + + /* + std::shared_ptr joints_play_client_; + std::shared_ptr gstt_srv_client_; + std::shared_ptr gtts_srv_client_; + std::shared_ptr chat_srv_client_; + */ + + rclcpp_action::ClientGoalHandle::SharedPtr head_goal_handle_; + rclcpp_action::ClientGoalHandle::SharedPtr eyes_goal_handle_; + rclcpp_action::ClientGoalHandle::SharedPtr ears_goal_handle_; + rclcpp_action::ClientGoalHandle::SharedPtr chest_goal_handle_; + + void eyesStatic(bool flag); + void earsStatic(bool flag); + void chestStatic(bool flag); + void headStatic(bool flag); + void headLoop(bool flag); + void earsLoop(bool flag); + void eyesLoop(bool flag); }; -} +} // namespace hni_chat_action_server -#endif // HNI_CPP__CHAT_ACTION_SERVER_HPP_ \ No newline at end of file +#endif // HNI_CPP__CHAT_ACTION_SERVER_HPP_ \ No newline at end of file diff --git a/hni_cpp/include/hni_cpp/chat_service_client.hpp b/hni_cpp/include/hni_cpp/chat_service_client.hpp index 931c075..410ef36 100644 --- a/hni_cpp/include/hni_cpp/chat_service_client.hpp +++ b/hni_cpp/include/hni_cpp/chat_service_client.hpp @@ -29,26 +29,21 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_components/register_node_macro.hpp" +namespace hni_chat_service_client +{ - -namespace hni_chat_service_client { - - -class ChatServiceClient : public rclcpp::Node { - - public: - explicit ChatServiceClient(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); +class ChatServiceClient : public rclcpp::Node +{ +public: + explicit ChatServiceClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); virtual ~ChatServiceClient(); - std::string sendSyncReq(std::string & phrase); - - private: + std::string sendSyncReq(std::string& phrase); +private: rclcpp::Client::SharedPtr client_ptr_; - }; +} // namespace hni_chat_service_client -} // namespace hni_gstt_service_client - -#endif //HNI_CPP__CHAT_SERVICE_CLIENT_HPP_ \ No newline at end of file +#endif // HNI_CPP__CHAT_SERVICE_CLIENT_HPP_ \ No newline at end of file diff --git a/hni_cpp/include/hni_cpp/gstt_service_client.hpp b/hni_cpp/include/hni_cpp/gstt_service_client.hpp index c788f98..d4c03ec 100644 --- a/hni_cpp/include/hni_cpp/gstt_service_client.hpp +++ b/hni_cpp/include/hni_cpp/gstt_service_client.hpp @@ -29,26 +29,21 @@ #include "std_srvs/srv/set_bool.hpp" +namespace hni_gstt_service_client +{ -namespace hni_gstt_service_client { - - -class GsttServiceClient : public rclcpp::Node { - - public: - explicit GsttServiceClient(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); +class GsttServiceClient : public rclcpp::Node +{ +public: + explicit GsttServiceClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); virtual ~GsttServiceClient(); std::string sendSyncReq(); - private: - +private: rclcpp::Client::SharedPtr client_ptr_; - }; - - } // namespace hni_gstt_service_client -#endif //HNI_CPP__GSTT_SERVICE_CLIENT_HPP_ \ No newline at end of file +#endif // HNI_CPP__GSTT_SERVICE_CLIENT_HPP_ \ No newline at end of file diff --git a/hni_cpp/include/hni_cpp/gtts_service_client.hpp b/hni_cpp/include/hni_cpp/gtts_service_client.hpp index 2b1a6da..e69a738 100644 --- a/hni_cpp/include/hni_cpp/gtts_service_client.hpp +++ b/hni_cpp/include/hni_cpp/gtts_service_client.hpp @@ -29,26 +29,21 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_components/register_node_macro.hpp" +namespace hni_gtts_service_client +{ - -namespace hni_gtts_service_client { - - -class GttsServiceClient : public rclcpp::Node { - - public: - explicit GttsServiceClient(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); +class GttsServiceClient : public rclcpp::Node +{ +public: + explicit GttsServiceClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); virtual ~GttsServiceClient(); - void sendSyncReq(std::string & text_to_speak); - - private: + void sendSyncReq(std::string& text_to_speak); +private: rclcpp::Client::SharedPtr client_ptr_; - }; +} // namespace hni_gtts_service_client -} // namespace hni_gstt_service_client - -#endif //HNI_CPP__GTTS_SERVICE_CLIENT_HPP_ \ No newline at end of file +#endif // HNI_CPP__GTTS_SERVICE_CLIENT_HPP_ \ No newline at end of file diff --git a/hni_cpp/include/hni_cpp/head_track_action_server.hpp b/hni_cpp/include/hni_cpp/head_track_action_server.hpp index 3b14854..f753a09 100644 --- a/hni_cpp/include/hni_cpp/head_track_action_server.hpp +++ b/hni_cpp/include/hni_cpp/head_track_action_server.hpp @@ -15,7 +15,6 @@ #ifndef HNI_CPP__HEAD_TRACK_ACTION_SERVER_HPP_ #define HNI_CPP__HEAD_TRACK_ACTION_SERVER_HPP_ - #include #include #include @@ -23,9 +22,9 @@ #include #include -#include // std::string, std::stof -#include // std::cout -#include // std::queue +#include // std::string, std::stof +#include // std::cout +#include // std::queue #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -39,69 +38,63 @@ #include "nao_lola_sensor_msgs/msg/joint_positions.hpp" #include "rclcpp_components/register_node_macro.hpp" - - -namespace hni_head_track_action_server { +namespace hni_head_track_action_server +{ using HeadTrack = hni_interfaces::action::VideoTracker; using GoalHandleHeadTrack = rclcpp_action::ServerGoalHandle; using ObjTrack = hni_interfaces::action::VideoTracker; using GoalHandleObjTrack = rclcpp_action::ClientGoalHandle; -class HeadTrackActionServer : public rclcpp::Node { - - public: - explicit HeadTrackActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); - virtual ~HeadTrackActionServer(); - - private: - - //objects - rclcpp_action::Server::SharedPtr action_server_; - rclcpp_action::Client::SharedPtr client_ptr_; - rclcpp::Publisher::SharedPtr jpos_pub_; - rclcpp::Publisher::SharedPtr jstiff_pub_; - rclcpp::Subscription::SharedPtr jpos_sub_; - - rclcpp::Subscription::SharedPtr obj_pos_sub_; - - rclcpp::TimerBase::SharedPtr timer_; - - nao_lola_command_msgs::msg::JointIndexes joint_indexes_msg_; - - std::queue x_track_; - std::queue y_track_; - float last_yaw_; - float last_pitch_; - const double kSecToHeadReset_; - const uint8_t kTrackMaxSize_; - const float kHeadWidthStep_;// = 0.36; - const float kHeadHeightStep_;// = 0.25; - const float kVerResolution_;// = 480; // y - const float kHorResolution_;// = 640; // x - - nao_lola_command_msgs::msg::JointPositions jpos_cmd_; - nao_lola_command_msgs::msg::JointStiffnesses jstiff_cmd_; - - bool fileSuccessfullyRead_; - - - void jposCallback(const nao_lola_sensor_msgs::msg::JointPositions & joints); - void sendGoal(); - void goalResponseCallback(const GoalHandleObjTrack::SharedPtr & goal_handle); - void feedbackCallback( GoalHandleObjTrack::SharedPtr, const std::shared_ptr feedback); - void resultCallback(const GoalHandleObjTrack::WrappedResult & result); - rclcpp_action::GoalResponse handleGoal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); - rclcpp_action::CancelResponse handleCancel( - const std::shared_ptr goal_handle); - void handleAccepted(const std::shared_ptr goal_handle); - void execute(const std::shared_ptr goal_handle); - - -}; //class - -}// namespace hni_head_track_action_server - -#endif // HNI_CPP__HEAD_TRACK_ACTION_SERVER_HPP_ \ No newline at end of file +class HeadTrackActionServer : public rclcpp::Node +{ +public: + explicit HeadTrackActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); + virtual ~HeadTrackActionServer(); + +private: + // objects + rclcpp_action::Server::SharedPtr action_server_; + rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::Publisher::SharedPtr jpos_pub_; + rclcpp::Publisher::SharedPtr jstiff_pub_; + rclcpp::Subscription::SharedPtr jpos_sub_; + + rclcpp::Subscription::SharedPtr obj_pos_sub_; + + rclcpp::TimerBase::SharedPtr timer_; + + nao_lola_command_msgs::msg::JointIndexes joint_indexes_msg_; + + std::queue x_track_; + std::queue y_track_; + float last_yaw_; + float last_pitch_; + const double kSecToHeadReset_; + const uint8_t kTrackMaxSize_; + const float kHeadWidthStep_; // = 0.36; + const float kHeadHeightStep_; // = 0.25; + const float kVerResolution_; // = 480; // y + const float kHorResolution_; // = 640; // x + + nao_lola_command_msgs::msg::JointPositions jpos_cmd_; + nao_lola_command_msgs::msg::JointStiffnesses jstiff_cmd_; + + bool fileSuccessfullyRead_; + + void jposCallback(const nao_lola_sensor_msgs::msg::JointPositions& joints); + void sendGoal(); + void goalResponseCallback(const GoalHandleObjTrack::SharedPtr& goal_handle); + void feedbackCallback(GoalHandleObjTrack::SharedPtr, const std::shared_ptr feedback); + void resultCallback(const GoalHandleObjTrack::WrappedResult& result); + rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse handleCancel(const std::shared_ptr goal_handle); + void handleAccepted(const std::shared_ptr goal_handle); + void execute(const std::shared_ptr goal_handle); + +}; // class + +} // namespace hni_head_track_action_server + +#endif // HNI_CPP__HEAD_TRACK_ACTION_SERVER_HPP_ \ No newline at end of file diff --git a/hni_cpp/include/hni_cpp/head_track_action_server2.hpp b/hni_cpp/include/hni_cpp/head_track_action_server2.hpp index 71f7cdf..2889e45 100644 --- a/hni_cpp/include/hni_cpp/head_track_action_server2.hpp +++ b/hni_cpp/include/hni_cpp/head_track_action_server2.hpp @@ -15,7 +15,6 @@ #ifndef HNI_CPP__HEAD_TRACK_ACTION_SERVER2_HPP_ #define HNI_CPP__HEAD_TRACK_ACTION_SERVER2_HPP_ - #include #include #include @@ -24,9 +23,9 @@ #include #include -#include // std::string, std::stof -#include // std::cout -#include // std::queue +#include // std::string, std::stof +#include // std::cout +#include // std::queue #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -41,71 +40,65 @@ #include "rclcpp_components/register_node_macro.hpp" #include "std_msgs/msg/string.hpp" - - -namespace hni_head_track_action_server2 { +namespace hni_head_track_action_server2 +{ using HeadTrack = hni_interfaces::action::VideoTracker; using GoalHandleHeadTrack = rclcpp_action::ServerGoalHandle; using ObjTrack = hni_interfaces::action::VideoTracker; using GoalHandleObjTrack = rclcpp_action::ClientGoalHandle; -class HeadTrackActionServer2 : public rclcpp::Node { - - public: - explicit HeadTrackActionServer2(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); - virtual ~HeadTrackActionServer2(); - - private: - - //objects - rclcpp_action::Server::SharedPtr action_server_; - rclcpp_action::Client::SharedPtr client_ptr_; - rclcpp::Publisher::SharedPtr jpos_pub_; - rclcpp::Publisher::SharedPtr jstiff_pub_; - rclcpp::Subscription::SharedPtr jpos_sub_; - - rclcpp::Subscription::SharedPtr obj_pos_sub_; - - rclcpp::TimerBase::SharedPtr timer_; - - nao_lola_command_msgs::msg::JointIndexes joint_indexes_msg_; - - std::queue x_track_; - std::queue y_track_; - float last_yaw_; - float last_pitch_; - const double kSecToHeadReset_; - const uint8_t kTrackMaxSize_; - const float kHeadWidthStep_;// = 0.36; - const float kHeadHeightStep_;// = 0.25; - const float kVerResolution_;// = 480; // y - const float kHorResolution_;// = 640; // x - - nao_lola_command_msgs::msg::JointPositions jpos_cmd_; - nao_lola_command_msgs::msg::JointStiffnesses jstiff_cmd_; - - bool fileSuccessfullyRead_; - std::map, std::string> head_pos_; - rclcpp::Publisher::SharedPtr publisher_; - - - void jposCallback(const nao_lola_sensor_msgs::msg::JointPositions & joints); - void sendGoal(); - void goalResponseCallback(const GoalHandleObjTrack::SharedPtr & goal_handle); - void feedbackCallback( GoalHandleObjTrack::SharedPtr, const std::shared_ptr feedback); - void resultCallback(const GoalHandleObjTrack::WrappedResult & result); - rclcpp_action::GoalResponse handleGoal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); - rclcpp_action::CancelResponse handleCancel( - const std::shared_ptr goal_handle); - void handleAccepted(const std::shared_ptr goal_handle); - void execute(const std::shared_ptr goal_handle); - - -}; //class - -}// namespace hni_head_track_action_server - -#endif // HNI_CPP__HEAD_TRACK_ACTION_SERVER2_HPP_ \ No newline at end of file +class HeadTrackActionServer2 : public rclcpp::Node +{ +public: + explicit HeadTrackActionServer2(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); + virtual ~HeadTrackActionServer2(); + +private: + // objects + rclcpp_action::Server::SharedPtr action_server_; + rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::Publisher::SharedPtr jpos_pub_; + rclcpp::Publisher::SharedPtr jstiff_pub_; + rclcpp::Subscription::SharedPtr jpos_sub_; + + rclcpp::Subscription::SharedPtr obj_pos_sub_; + + rclcpp::TimerBase::SharedPtr timer_; + + nao_lola_command_msgs::msg::JointIndexes joint_indexes_msg_; + + std::queue x_track_; + std::queue y_track_; + float last_yaw_; + float last_pitch_; + const double kSecToHeadReset_; + const uint8_t kTrackMaxSize_; + const float kHeadWidthStep_; // = 0.36; + const float kHeadHeightStep_; // = 0.25; + const float kVerResolution_; // = 480; // y + const float kHorResolution_; // = 640; // x + + nao_lola_command_msgs::msg::JointPositions jpos_cmd_; + nao_lola_command_msgs::msg::JointStiffnesses jstiff_cmd_; + + bool fileSuccessfullyRead_; + std::map, std::string> head_pos_; + rclcpp::Publisher::SharedPtr publisher_; + + void jposCallback(const nao_lola_sensor_msgs::msg::JointPositions& joints); + void sendGoal(); + void goalResponseCallback(const GoalHandleObjTrack::SharedPtr& goal_handle); + void feedbackCallback(GoalHandleObjTrack::SharedPtr, const std::shared_ptr feedback); + void resultCallback(const GoalHandleObjTrack::WrappedResult& result); + rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse handleCancel(const std::shared_ptr goal_handle); + void handleAccepted(const std::shared_ptr goal_handle); + void execute(const std::shared_ptr goal_handle); + +}; // class + +} // namespace hni_head_track_action_server2 + +#endif // HNI_CPP__HEAD_TRACK_ACTION_SERVER2_HPP_ \ No newline at end of file diff --git a/hni_cpp/include/hni_cpp/indexes.hpp b/hni_cpp/include/hni_cpp/indexes.hpp index bf57ad3..3119c5f 100644 --- a/hni_cpp/include/hni_cpp/indexes.hpp +++ b/hni_cpp/include/hni_cpp/indexes.hpp @@ -23,31 +23,20 @@ namespace indexes { static const std::vector indexes = { - nao_lola_command_msgs::msg::JointIndexes::HEADYAW, - nao_lola_command_msgs::msg::JointIndexes::HEADPITCH, - nao_lola_command_msgs::msg::JointIndexes::LSHOULDERPITCH, - nao_lola_command_msgs::msg::JointIndexes::LSHOULDERROLL, - nao_lola_command_msgs::msg::JointIndexes::LELBOWYAW, - nao_lola_command_msgs::msg::JointIndexes::LELBOWROLL, - nao_lola_command_msgs::msg::JointIndexes::LWRISTYAW, - nao_lola_command_msgs::msg::JointIndexes::LHIPYAWPITCH, - nao_lola_command_msgs::msg::JointIndexes::LHIPROLL, - nao_lola_command_msgs::msg::JointIndexes::LHIPPITCH, - nao_lola_command_msgs::msg::JointIndexes::LKNEEPITCH, - nao_lola_command_msgs::msg::JointIndexes::LANKLEPITCH, - nao_lola_command_msgs::msg::JointIndexes::LANKLEROLL, - nao_lola_command_msgs::msg::JointIndexes::RHIPROLL, - nao_lola_command_msgs::msg::JointIndexes::RHIPPITCH, - nao_lola_command_msgs::msg::JointIndexes::RKNEEPITCH, - nao_lola_command_msgs::msg::JointIndexes::RANKLEPITCH, - nao_lola_command_msgs::msg::JointIndexes::RANKLEROLL, - nao_lola_command_msgs::msg::JointIndexes::RSHOULDERPITCH, - nao_lola_command_msgs::msg::JointIndexes::RSHOULDERROLL, - nao_lola_command_msgs::msg::JointIndexes::RELBOWYAW, - nao_lola_command_msgs::msg::JointIndexes::RELBOWROLL, - nao_lola_command_msgs::msg::JointIndexes::RWRISTYAW, - nao_lola_command_msgs::msg::JointIndexes::LHAND, - nao_lola_command_msgs::msg::JointIndexes::RHAND}; + nao_lola_command_msgs::msg::JointIndexes::HEADYAW, nao_lola_command_msgs::msg::JointIndexes::HEADPITCH, + nao_lola_command_msgs::msg::JointIndexes::LSHOULDERPITCH, nao_lola_command_msgs::msg::JointIndexes::LSHOULDERROLL, + nao_lola_command_msgs::msg::JointIndexes::LELBOWYAW, nao_lola_command_msgs::msg::JointIndexes::LELBOWROLL, + nao_lola_command_msgs::msg::JointIndexes::LWRISTYAW, nao_lola_command_msgs::msg::JointIndexes::LHIPYAWPITCH, + nao_lola_command_msgs::msg::JointIndexes::LHIPROLL, nao_lola_command_msgs::msg::JointIndexes::LHIPPITCH, + nao_lola_command_msgs::msg::JointIndexes::LKNEEPITCH, nao_lola_command_msgs::msg::JointIndexes::LANKLEPITCH, + nao_lola_command_msgs::msg::JointIndexes::LANKLEROLL, nao_lola_command_msgs::msg::JointIndexes::RHIPROLL, + nao_lola_command_msgs::msg::JointIndexes::RHIPPITCH, nao_lola_command_msgs::msg::JointIndexes::RKNEEPITCH, + nao_lola_command_msgs::msg::JointIndexes::RANKLEPITCH, nao_lola_command_msgs::msg::JointIndexes::RANKLEROLL, + nao_lola_command_msgs::msg::JointIndexes::RSHOULDERPITCH, nao_lola_command_msgs::msg::JointIndexes::RSHOULDERROLL, + nao_lola_command_msgs::msg::JointIndexes::RELBOWYAW, nao_lola_command_msgs::msg::JointIndexes::RELBOWROLL, + nao_lola_command_msgs::msg::JointIndexes::RWRISTYAW, nao_lola_command_msgs::msg::JointIndexes::LHAND, + nao_lola_command_msgs::msg::JointIndexes::RHAND +}; } // namespace indexes diff --git a/hni_cpp/include/hni_cpp/joints_play_action_client.hpp b/hni_cpp/include/hni_cpp/joints_play_action_client.hpp index 2ce1d77..0b84e71 100644 --- a/hni_cpp/include/hni_cpp/joints_play_action_client.hpp +++ b/hni_cpp/include/hni_cpp/joints_play_action_client.hpp @@ -30,37 +30,33 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_components/register_node_macro.hpp" -//namespace fs = boost::filesystem; +// namespace fs = boost::filesystem; - -namespace hni_joints_play_action_client { +namespace hni_joints_play_action_client +{ using JointsPlay = hni_interfaces::action::JointsPlay; using GoalHandleJointsPlay = rclcpp_action::ClientGoalHandle; -class JointsPlayActionClient: public rclcpp::Node { - - public: - explicit JointsPlayActionClient(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); - virtual ~JointsPlayActionClient(); - - void sendAsyncGoal(std::string & action_path); - //void sendGoal(); +class JointsPlayActionClient : public rclcpp::Node +{ +public: + explicit JointsPlayActionClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); + virtual ~JointsPlayActionClient(); - private: + void sendAsyncGoal(std::string& action_path); + // void sendGoal(); - rclcpp_action::Client::SharedPtr client_ptr_; - rclcpp::TimerBase::SharedPtr timer_; - - std::string getDefaultFullFilePath(); - void goalResponseCallback(const GoalHandleJointsPlay::SharedPtr & goal_handle); - void feedbackCallback( GoalHandleJointsPlay::SharedPtr, - const std::shared_ptr feedback); - void resultCallback(const GoalHandleJointsPlay::WrappedResult & result); +private: + rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::TimerBase::SharedPtr timer_; + std::string getDefaultFullFilePath(); + void goalResponseCallback(const GoalHandleJointsPlay::SharedPtr& goal_handle); + void feedbackCallback(GoalHandleJointsPlay::SharedPtr, const std::shared_ptr feedback); + void resultCallback(const GoalHandleJointsPlay::WrappedResult& result); }; -} // namespace hni_joints_play_action_client - +} // namespace hni_joints_play_action_client -#endif //HNI_CPP__JOINTS_PLAY_ACTION_CLIENT_HPP_ \ No newline at end of file +#endif // HNI_CPP__JOINTS_PLAY_ACTION_CLIENT_HPP_ \ No newline at end of file diff --git a/hni_cpp/include/hni_cpp/joints_play_action_server.hpp b/hni_cpp/include/hni_cpp/joints_play_action_server.hpp index b18be2a..a9cb16e 100644 --- a/hni_cpp/include/hni_cpp/joints_play_action_server.hpp +++ b/hni_cpp/include/hni_cpp/joints_play_action_server.hpp @@ -17,15 +17,15 @@ #include #include -#include // std::thread, std::this_thread::yield -#include // std::atomic, std::atomic_flag, ATOMIC_FLAG_INIT +#include // std::thread, std::this_thread::yield +#include // std::atomic, std::atomic_flag, ATOMIC_FLAG_INIT #include #include #include #include -#include // std::string, std::stof std::c_str +#include // std::string, std::stof std::c_str -#include // std::cout +#include // std::cout #include "hni_interfaces/action/joints_play.hpp" #include "rclcpp/rclcpp.hpp" @@ -35,43 +35,39 @@ #include "nao_lola_command_msgs/msg/joint_indexes.hpp" #include "nao_lola_command_msgs/msg/joint_stiffnesses.hpp" -namespace hni_joints_play_action_server { +namespace hni_joints_play_action_server +{ using JointsPlay = hni_interfaces::action::JointsPlay; using GoalHandleJointsPlay = rclcpp_action::ServerGoalHandle; -class JointsPlayActionServer : public rclcpp::Node { - - public: - explicit JointsPlayActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); - virtual ~JointsPlayActionServer(); - - private: +class JointsPlayActionServer : public rclcpp::Node +{ +public: + explicit JointsPlayActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions{}); + virtual ~JointsPlayActionServer(); +private: rclcpp_action::Server::SharedPtr action_server_; rclcpp::Publisher::SharedPtr jpos_pub_; rclcpp::Publisher::SharedPtr jstiff_pub_; std::ifstream ifs_; std::vector recorded_joints_; - + nao_lola_command_msgs::msg::JointPositions jpos_cmd_; nao_lola_command_msgs::msg::JointStiffnesses jstiff_cmd_; bool fileSuccessfullyRead_; std::atomic free_; - - rclcpp_action::GoalResponse handleGoal( const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); - rclcpp_action::CancelResponse handleCancel( const std::shared_ptr goal_handle); + rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse handleCancel(const std::shared_ptr goal_handle); void handleAccepted(const std::shared_ptr goal_handle); void execute(const std::shared_ptr goal_handle); - - }; -} // namespace hni_joints_play_action_server - +} // namespace hni_joints_play_action_server #endif // HNI_CPP__JOINTS_PLAY_ACTION_SERVER_HPP_ \ No newline at end of file diff --git a/hni_cpp/package.xml b/hni_cpp/package.xml index 65d7e77..2c7b267 100644 --- a/hni_cpp/package.xml +++ b/hni_cpp/package.xml @@ -9,6 +9,10 @@ ament_cmake + ament_lint_auto + ament_lint_common + ament_cmake_clang_format + rclcpp rclcpp_action rclcpp_components @@ -21,8 +25,7 @@ hni_interfaces rcutils - ament_lint_auto - ament_lint_common + ament_cmake diff --git a/hni_cpp/src/chat_action_client.cpp b/hni_cpp/src/chat_action_client.cpp index eb91026..f423201 100644 --- a/hni_cpp/src/chat_action_client.cpp +++ b/hni_cpp/src/chat_action_client.cpp @@ -1,13 +1,13 @@ #include #include -#include // std::cout +#include // std::cout #include -#include // std::string, std::stof +#include // std::string, std::stof #include #include -//#include +// #include -//#include +// #include #include "hni_interfaces/action/chat_play.hpp" @@ -18,95 +18,97 @@ #include "hni_cpp/chat_action_client.hpp" -namespace hni_chat_action_client { - -ChatActionClient::ChatActionClient(const rclcpp::NodeOptions & options) - : rclcpp::Node("chat_action_client_node", options) { +namespace hni_chat_action_client +{ +ChatActionClient::ChatActionClient(const rclcpp::NodeOptions& options) + : rclcpp::Node("chat_action_client_node", options) +{ using namespace std::placeholders; - this->client_ptr_ = rclcpp_action::create_client( - this, - "chat_play"); - - this->timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&ChatActionClient::sendAsyncGoal, this)); + this->client_ptr_ = rclcpp_action::create_client(this, "chat_play"); + this->timer_ = + this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&ChatActionClient::sendAsyncGoal, this)); RCLCPP_INFO(this->get_logger(), "ChatActionClient initialized"); - } -ChatActionClient::~ChatActionClient() {} +ChatActionClient::~ChatActionClient() +{ +} -void ChatActionClient::sendAsyncGoal() { +void ChatActionClient::sendAsyncGoal() +{ using namespace std::placeholders; this->timer_->cancel(); - if (!this->client_ptr_->wait_for_action_server()) { + if (!this->client_ptr_->wait_for_action_server()) + { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } auto goal_msg = hni_interfaces::action::ChatPlay::Goal(); - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&ChatActionClient::goalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&ChatActionClient::goalResponseCallback, this, _1); - //send_goal_options.feedback_callback = - // std::bind(&ChatActionClient::feedbackCallback, this, _1, _2); + // send_goal_options.feedback_callback = + // std::bind(&ChatActionClient::feedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&ChatActionClient::resultCallback, this, _1); + send_goal_options.result_callback = std::bind(&ChatActionClient::resultCallback, this, _1); - RCLCPP_INFO(this->get_logger(), "Sending goal: " ); + RCLCPP_INFO(this->get_logger(), "Sending goal: "); this->client_ptr_->async_send_goal(goal_msg, send_goal_options); } - void ChatActionClient::goalResponseCallback( - const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle) { - if (!goal_handle) { + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) +{ + if (!goal_handle) + { RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } else { + } + else + { RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); } } void ChatActionClient::feedbackCallback( - rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback) { - - //TODO - + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) +{ + // TODO } void ChatActionClient::resultCallback( - const rclcpp_action::ClientGoalHandle::WrappedResult & result) { - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; + const rclcpp_action::ClientGoalHandle::WrappedResult& result) +{ + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; } - //if (result.result->success) + // if (result.result->success) RCLCPP_INFO(this->get_logger(), "Joints posisitions regulary played."); - //rclcpp::shutdown(); + // rclcpp::shutdown(); } -} // hni_chat_action_client +} // namespace hni_chat_action_client RCLCPP_COMPONENTS_REGISTER_NODE(hni_chat_action_client::ChatActionClient) \ No newline at end of file diff --git a/hni_cpp/src/chat_action_server.cpp b/hni_cpp/src/chat_action_server.cpp index e73b9b3..2092131 100644 --- a/hni_cpp/src/chat_action_server.cpp +++ b/hni_cpp/src/chat_action_server.cpp @@ -14,20 +14,19 @@ #include #include -#include // std::isalpha +#include // std::isalpha #include #include #include #include #include -#include //std::stringstream +#include //std::stringstream #include #include #include #include #include - #include "nao_lola_command_msgs/msg/head_leds.hpp" #include "nao_lola_command_msgs/msg/right_eye_leds.hpp" #include "nao_lola_command_msgs/msg/left_eye_leds.hpp" @@ -43,745 +42,796 @@ #include "hni_interfaces/action/joints_play.hpp" #include "hni_interfaces/action/chat_play.hpp" - #include "hni_cpp/chat_action_server.hpp" //#include "hni_cpp/joints_play_action_client.hpp" //#include "nao_led_server/led_action_server.hpp" - #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "std_srvs/srv/set_bool.hpp" - -namespace hni_chat_action_server { +namespace hni_chat_action_server +{ using namespace std::chrono_literals; +ChatActionServer::ChatActionServer(const rclcpp::NodeOptions& options) + : rclcpp::Node("chat_action_server_node", options), kSecPerWord_(0.5), kForwardParam_(10) +{ + using namespace std::placeholders; -ChatActionServer::ChatActionServer(const rclcpp::NodeOptions & options) - : rclcpp::Node("chat_action_server_node", options), kSecPerWord_(0.5), kForwardParam_(10) { - using namespace std::placeholders; - - - this->gstt_srv_client_ = this->create_client("gstt_service"); - - this->gtts_srv_client_ = this->create_client("gtts_service"); - - this->chat_srv_client_ = this->create_client("chatGPT_service"); + this->gstt_srv_client_ = this->create_client("gstt_service"); - this->leds_play_act_client_ = rclcpp_action::create_client(this, "leds_play"); + this->gtts_srv_client_ = this->create_client("gtts_service"); - this->joints_act_client_ = rclcpp_action::create_client( - this, "joints_play"); + this->chat_srv_client_ = this->create_client("chatGPT_service"); - this->action_server_ = rclcpp_action::create_server( - this, - "chat_play", - std::bind(&ChatActionServer::handleGoal, this, _1, _2), - std::bind(&ChatActionServer::handleCancel, this, _1), - std::bind(&ChatActionServer::handleAccepted, this, _1)); + this->leds_play_act_client_ = rclcpp_action::create_client(this, "leds_play"); + this->joints_act_client_ = rclcpp_action::create_client(this, "joints_play"); - moves_map_["hello"] = "install/hni_cpp/include/moves/hello.txt"; - moves_map_["hi"] = "install/hni_cpp/include/moves/hello.txt"; - moves_map_["bye"] = "install/hni_cpp/include/moves/hello.txt"; - moves_map_["byebye"] = "install/hni_cpp/include/moves/hello.txt"; - moves_map_["goodbye"] = "install/hni_cpp/include/moves/hello.txt"; - moves_map_["good-bye"] = "install/hni_cpp/include/moves/hello.txt"; - moves_map_["big"] = "install/hni_cpp/include/moves/big.txt"; - moves_map_["little"] = "install/hni_cpp/include/moves/little.txt"; - moves_map_["down"] = "install/hni_cpp/include/moves/down.txt"; - moves_map_["up"] = "install/hni_cpp/include/moves/up.txt"; - moves_map_["right"] = "install/hni_cpp/include/moves/right.txt"; - moves_map_["left"] = "install/hni_cpp/include/moves/left.txt"; - moves_map_["fear"] = "install/hni_cpp/include/moves/fear.txt"; - moves_map_["scared"] = "install/hni_cpp/include/moves/fear.txt"; + this->action_server_ = rclcpp_action::create_server( + this, "chat_play", std::bind(&ChatActionServer::handleGoal, this, _1, _2), + std::bind(&ChatActionServer::handleCancel, this, _1), std::bind(&ChatActionServer::handleAccepted, this, _1)); - RCLCPP_INFO(this->get_logger(), "ChatActionServer Initialized"); + moves_map_["hello"] = "install/hni_cpp/include/moves/hello.txt"; + moves_map_["hi"] = "install/hni_cpp/include/moves/hello.txt"; + moves_map_["bye"] = "install/hni_cpp/include/moves/hello.txt"; + moves_map_["byebye"] = "install/hni_cpp/include/moves/hello.txt"; + moves_map_["goodbye"] = "install/hni_cpp/include/moves/hello.txt"; + moves_map_["good-bye"] = "install/hni_cpp/include/moves/hello.txt"; + moves_map_["big"] = "install/hni_cpp/include/moves/big.txt"; + moves_map_["little"] = "install/hni_cpp/include/moves/little.txt"; + moves_map_["down"] = "install/hni_cpp/include/moves/down.txt"; + moves_map_["up"] = "install/hni_cpp/include/moves/up.txt"; + moves_map_["right"] = "install/hni_cpp/include/moves/right.txt"; + moves_map_["left"] = "install/hni_cpp/include/moves/left.txt"; + moves_map_["fear"] = "install/hni_cpp/include/moves/fear.txt"; + moves_map_["scared"] = "install/hni_cpp/include/moves/fear.txt"; + RCLCPP_INFO(this->get_logger(), "ChatActionServer Initialized"); } -ChatActionServer::~ChatActionServer() {} - +ChatActionServer::~ChatActionServer() +{ +} rclcpp_action::GoalResponse ChatActionServer::handleGoal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) { - - RCLCPP_INFO(this->get_logger(), "Received chat goal request"); - (void)uuid; - - while (!gstt_srv_client_->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for gstt_service. Exiting."); - return rclcpp_action::GoalResponse::REJECT; - } - RCLCPP_INFO(this->get_logger(), "gstt_service not available, waiting again..."); + const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) +{ + RCLCPP_INFO(this->get_logger(), "Received chat goal request"); + (void)uuid; + + while (!gstt_srv_client_->wait_for_service(1s)) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for gstt_service. Exiting."); + return rclcpp_action::GoalResponse::REJECT; } - - while (!gtts_srv_client_->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for gtts_service. Exiting."); - return rclcpp_action::GoalResponse::REJECT; - } - RCLCPP_INFO(this->get_logger(), "gtts_service not available, waiting again..."); + RCLCPP_INFO(this->get_logger(), "gstt_service not available, waiting again..."); + } + + while (!gtts_srv_client_->wait_for_service(1s)) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for gtts_service. Exiting."); + return rclcpp_action::GoalResponse::REJECT; } - - while (!chat_srv_client_->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for chat_service. Exiting."); - return rclcpp_action::GoalResponse::REJECT; - } - RCLCPP_INFO(this->get_logger(), "chat_service not available, waiting again..."); + RCLCPP_INFO(this->get_logger(), "gtts_service not available, waiting again..."); + } + + while (!chat_srv_client_->wait_for_service(1s)) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for chat_service. Exiting."); + return rclcpp_action::GoalResponse::REJECT; } - - while (!joints_act_client_->wait_for_action_server(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for joints_play action server. Exiting."); - return rclcpp_action::GoalResponse::REJECT; - } - RCLCPP_INFO(this->get_logger(), "joints_play action server not available, waiting again..."); + RCLCPP_INFO(this->get_logger(), "chat_service not available, waiting again..."); + } + + while (!joints_act_client_->wait_for_action_server(1s)) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for joints_play action server. Exiting."); + return rclcpp_action::GoalResponse::REJECT; } + RCLCPP_INFO(this->get_logger(), "joints_play action server not available, waiting again..."); + } - - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } rclcpp_action::CancelResponse ChatActionServer::handleCancel( - const std::shared_ptr> goal_handle) { - RCLCPP_INFO(this->get_logger(), "Received request to cancel chat goal"); - (void)goal_handle; - return rclcpp_action::CancelResponse::ACCEPT; + const std::shared_ptr> goal_handle) +{ + RCLCPP_INFO(this->get_logger(), "Received request to cancel chat goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; } void ChatActionServer::handleAccepted( - const std::shared_ptr> goal_handle) { - using namespace std::placeholders; - // this needs to return quickly to avoid blocking the executor, so spin up a new thread - std::thread{std::bind(&ChatActionServer::execute, this, _1), goal_handle} .detach(); + const std::shared_ptr> goal_handle) +{ + using namespace std::placeholders; + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{ std::bind(&ChatActionServer::execute, this, _1), goal_handle }.detach(); } void ChatActionServer::execute( - const std::shared_ptr> goal_handle) { - using namespace std::placeholders; - - RCLCPP_INFO(this->get_logger(), "Executing chat goal"); - - auto chat_goal = goal_handle->get_goal(); - auto chat_feedback = std::make_shared(); - auto chat_result = std::make_shared(); - - auto gstt_request = std::make_shared(); - gstt_request->data = true; - - unsigned num_words = 0; - bool first_word = true; - std::vector words; - std::vector key_words; - std::vector key_words_time; - bool playing = false, firstLoop = true; - double last_action_time; - std::string recognized_speach; - std::string chatgpt_answer; - std::string word; - std::string user_input; - std::string action_path; - double t_start; - double t_key_word; - double t_word; - double t_cur; - double t_sleep; - - while (rclcpp::ok()) { - - if (goal_handle->is_canceling()) { - chat_result->success = true; - goal_handle->canceled(chat_result); - RCLCPP_INFO(this->get_logger(), "Chat Goal canceled"); - return; - } - - if (!firstLoop) { - - // stt service - RCLCPP_INFO(this->get_logger(), "Ready to listen"); - - this->eyesStatic(true); - this->earsLoop(true); - - //TTS - RCLCPP_WARN(this->get_logger(), "sending gstt request" ); - auto gstt_future = gstt_srv_client_->async_send_request(gstt_request); - RCLCPP_INFO(this->get_logger(), "gstt request sent" ); - auto gstt_result = gstt_future.get(); // wait for the result - RCLCPP_INFO(this->get_logger(), "gstt gets result %d", gstt_result.get()->success ); - - this->earsLoop(false); - bool stt_ok = gstt_result.get()->success; - - if (stt_ok) { - - recognized_speach = gstt_result.get()->message; - RCLCPP_INFO(this->get_logger(), ("Recognized speech: " + recognized_speach).c_str()); - - - this->headLoop(true); - - // chatgpt service - auto chat_request = std::make_shared(); - chat_request->question = recognized_speach; - auto chat_future = chat_srv_client_->async_send_request(chat_request); - auto chat_result = chat_future.get(); // wait for the result - - this->headLoop(false); - - chatgpt_answer = chat_result.get()->answer; - RCLCPP_INFO(this->get_logger(), ("chatGPT answer: " + chatgpt_answer).c_str()); - - - // text and timing analysis of the answer - num_words = 0; - words.clear(); - key_words.clear(); - key_words_time.clear(); - std::istringstream iss_ans(chatgpt_answer); - first_word = true; - - while (iss_ans >> word) { - std::string alphaAndApostropheOnly; - - for (auto it = word.begin(); it != word.end(); ++it) { - char ch = *it; - if (std::isalpha(ch) || ch == '\'') { // Include apostrophes - alphaAndApostropheOnly += std::tolower(ch); - } - } - - if (!alphaAndApostropheOnly.empty()) { - words.push_back(alphaAndApostropheOnly); - } - } - - std::stringstream ss0; - for (const auto& s : words) { - ss0 << s << "; "; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "words: " << ss0.str() ); - for (std::string w : words) { - num_words += 1; - std::transform(w.begin(), w.end(), w.begin(), - [](unsigned char c) { return std::tolower(c); }); - if (moves_map_.find(w) != moves_map_.end()) { - key_words.push_back(w); - if (num_words <= kForwardParam_ && first_word) { - key_words_time.push_back(0.1); - first_word = false; - } else if (num_words <= kForwardParam_ && !first_word) { - key_words_time.push_back(num_words * kSecPerWord_); - } else { - key_words_time.push_back((num_words - kForwardParam_)*kSecPerWord_); - } - } - - } - - std::stringstream ss1; - for (auto it = key_words.begin(); it != key_words.end(); it++) { - if (it != key_words.begin()) { - ss1 << " "; - } - ss1 << *it; - } - RCLCPP_DEBUG_STREAM(this->get_logger(), "key_words: " << ss1.str() ); - - std::stringstream ss2; - for (auto it = key_words_time.begin(); it != key_words_time.end(); it++) { - if (it != key_words_time.begin()) { - ss2 << " "; - } - ss2 << *it; - } - RCLCPP_DEBUG_STREAM(this->get_logger(), "key_words_time: " << ss2.str() ); - - }//stt ok - - - // speaking TTS - - this->earsStatic(true); - this->eyesLoop(true); - - //gtts_srv_client_->sendSyncReq(chatgpt_answer); - auto gtts_request = std::make_shared(); - - if (stt_ok) { - gtts_request->text = chatgpt_answer; - } else { - gtts_request->text = "I am sorry, I do not understand. Could you repeat, please?"; - } - - auto gtts_future = gtts_srv_client_->async_send_request(gtts_request); + const std::shared_ptr> goal_handle) +{ + using namespace std::placeholders; + + RCLCPP_INFO(this->get_logger(), "Executing chat goal"); + + auto chat_goal = goal_handle->get_goal(); + auto chat_feedback = std::make_shared(); + auto chat_result = std::make_shared(); + + auto gstt_request = std::make_shared(); + gstt_request->data = true; + + unsigned num_words = 0; + bool first_word = true; + std::vector words; + std::vector key_words; + std::vector key_words_time; + bool playing = false, firstLoop = true; + double last_action_time; + std::string recognized_speach; + std::string chatgpt_answer; + std::string word; + std::string user_input; + std::string action_path; + double t_start; + double t_key_word; + double t_word; + double t_cur; + double t_sleep; + + while (rclcpp::ok()) + { + if (goal_handle->is_canceling()) + { + chat_result->success = true; + goal_handle->canceled(chat_result); + RCLCPP_INFO(this->get_logger(), "Chat Goal canceled"); + return; + } - auto gtts_result = gtts_future.get(); // Wait for the result. - if (gtts_result.get()->success) { - RCLCPP_DEBUG(this->get_logger(), "tts request completed: %d", gtts_result.get()->success); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to call gtts_service"); - return; + if (!firstLoop) + { + // stt service + RCLCPP_INFO(this->get_logger(), "Ready to listen"); + + this->eyesStatic(true); + this->earsLoop(true); + + // TTS + RCLCPP_WARN(this->get_logger(), "sending gstt request"); + auto gstt_future = gstt_srv_client_->async_send_request(gstt_request); + RCLCPP_INFO(this->get_logger(), "gstt request sent"); + auto gstt_result = gstt_future.get(); // wait for the result + RCLCPP_INFO(this->get_logger(), "gstt gets result %d", gstt_result.get()->success); + + this->earsLoop(false); + bool stt_ok = gstt_result.get()->success; + + if (stt_ok) + { + recognized_speach = gstt_result.get()->message; + RCLCPP_INFO(this->get_logger(), ("Recognized speech: " + recognized_speach).c_str()); + + this->headLoop(true); + + // chatgpt service + auto chat_request = std::make_shared(); + chat_request->question = recognized_speach; + auto chat_future = chat_srv_client_->async_send_request(chat_request); + auto chat_result = chat_future.get(); // wait for the result + + this->headLoop(false); + + chatgpt_answer = chat_result.get()->answer; + RCLCPP_INFO(this->get_logger(), ("chatGPT answer: " + chatgpt_answer).c_str()); + + // text and timing analysis of the answer + num_words = 0; + words.clear(); + key_words.clear(); + key_words_time.clear(); + std::istringstream iss_ans(chatgpt_answer); + first_word = true; + + while (iss_ans >> word) + { + std::string alphaAndApostropheOnly; + + for (auto it = word.begin(); it != word.end(); ++it) + { + char ch = *it; + if (std::isalpha(ch) || ch == '\'') + { // Include apostrophes + alphaAndApostropheOnly += std::tolower(ch); } + } - if (stt_ok) { - - //play moves - t_start = this->now().seconds(); - //rclcpp::sleep_for(std::chrono::milliseconds(50)); - - - for (unsigned i = 0; i < key_words.size(); ++i) { - - t_key_word = key_words_time[i]; - t_word = t_key_word + t_start; - t_cur = this->now().seconds(); - - if (t_cur < t_word) { - //wait - t_sleep = t_word - t_cur; - rclcpp::sleep_for(std::chrono::nanoseconds(static_cast(t_sleep * 1e9))); - - //execute move - action_path = moves_map_[key_words[i]]; - - auto goal_msg = hni_interfaces::action::JointsPlay::Goal(); - goal_msg.path = action_path; + if (!alphaAndApostropheOnly.empty()) + { + words.push_back(alphaAndApostropheOnly); + } + } - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + std::stringstream ss0; + for (const auto& s : words) + { + ss0 << s << "; "; + } - send_goal_options.goal_response_callback = - std::bind(&ChatActionServer::jointsPlayGoalResponseCallback, this, std::placeholders::_1); - //send_goal_options.feedback_callback = - // std::bind(&ChatActionServer::jointsPlayFeedbackCallback, this, std::placeholders::_1, std::placeholders::_2); - send_goal_options.result_callback = - std::bind(&ChatActionServer::jointsPlayResultCallback, this, std::placeholders::_1); + RCLCPP_INFO_STREAM(this->get_logger(), "words: " << ss0.str()); + for (std::string w : words) + { + num_words += 1; + std::transform(w.begin(), w.end(), w.begin(), [](unsigned char c) { return std::tolower(c); }); + if (moves_map_.find(w) != moves_map_.end()) + { + key_words.push_back(w); + if (num_words <= kForwardParam_ && first_word) + { + key_words_time.push_back(0.1); + first_word = false; + } + else if (num_words <= kForwardParam_ && !first_word) + { + key_words_time.push_back(num_words * kSecPerWord_); + } + else + { + key_words_time.push_back((num_words - kForwardParam_) * kSecPerWord_); + } + } + } - RCLCPP_DEBUG(this->get_logger(), (" jointsPlay Sending goal: " + action_path).c_str() ); + std::stringstream ss1; + for (auto it = key_words.begin(); it != key_words.end(); it++) + { + if (it != key_words.begin()) + { + ss1 << " "; + } + ss1 << *it; + } + RCLCPP_DEBUG_STREAM(this->get_logger(), "key_words: " << ss1.str()); + + std::stringstream ss2; + for (auto it = key_words_time.begin(); it != key_words_time.end(); it++) + { + if (it != key_words_time.begin()) + { + ss2 << " "; + } + ss2 << *it; + } + RCLCPP_DEBUG_STREAM(this->get_logger(), "key_words_time: " << ss2.str()); - this->joints_act_client_->async_send_goal(goal_msg, send_goal_options); + } // stt ok - } + // speaking TTS - } + this->earsStatic(true); + this->eyesLoop(true); - } + // gtts_srv_client_->sendSyncReq(chatgpt_answer); + auto gtts_request = std::make_shared(); - //user input - std::cout << "Press after your listening is finished." << std::endl; - std::getline(std::cin, user_input); - this->eyesLoop(false); + if (stt_ok) + { + gtts_request->text = chatgpt_answer; + } + else + { + gtts_request->text = "I am sorry, I do not understand. Could you repeat, please?"; + } + auto gtts_future = gtts_srv_client_->async_send_request(gtts_request); - } else { - firstLoop = false; - //user input - std::cout << "Press to START" << std::endl; - std::getline(std::cin, user_input); - this->chestStatic(true); + auto gtts_result = gtts_future.get(); // Wait for the result. + if (gtts_result.get()->success) + { + RCLCPP_DEBUG(this->get_logger(), "tts request completed: %d", gtts_result.get()->success); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to call gtts_service"); + return; + } + + if (stt_ok) + { + // play moves + t_start = this->now().seconds(); + // rclcpp::sleep_for(std::chrono::milliseconds(50)); + + for (unsigned i = 0; i < key_words.size(); ++i) + { + t_key_word = key_words_time[i]; + t_word = t_key_word + t_start; + t_cur = this->now().seconds(); + + if (t_cur < t_word) + { + // wait + t_sleep = t_word - t_cur; + rclcpp::sleep_for(std::chrono::nanoseconds(static_cast(t_sleep * 1e9))); + + // execute move + action_path = moves_map_[key_words[i]]; + + auto goal_msg = hni_interfaces::action::JointsPlay::Goal(); + goal_msg.path = action_path; + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + send_goal_options.goal_response_callback = + std::bind(&ChatActionServer::jointsPlayGoalResponseCallback, this, std::placeholders::_1); + // send_goal_options.feedback_callback = + // std::bind(&ChatActionServer::jointsPlayFeedbackCallback, this, std::placeholders::_1, + // std::placeholders::_2); + send_goal_options.result_callback = + std::bind(&ChatActionServer::jointsPlayResultCallback, this, std::placeholders::_1); + + RCLCPP_DEBUG(this->get_logger(), (" jointsPlay Sending goal: " + action_path).c_str()); + + this->joints_act_client_->async_send_goal(goal_msg, send_goal_options); + } } + } + // user input + std::cout << "Press after your listening is finished." << std::endl; + std::getline(std::cin, user_input); + this->eyesLoop(false); + } + else + { + firstLoop = false; + // user input + std::cout << "Press to START" << std::endl; + std::getline(std::cin, user_input); + this->chestStatic(true); + } + } // execute while(rclcpp::ok()) - }//execute while(rclcpp::ok()) - -}// execute +} // execute /*############## LEDS PLAY ACTION CLIENT ##############*/ void ChatActionServer::ledsPlayGoalResponseCallback( - const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle) { - if (!goal_handle) { - RCLCPP_ERROR(this->get_logger(), "ledsPlay Goal was rejected by server"); - } else { - RCLCPP_INFO(this->get_logger(), "ledsPlay Goal accepted by server, waiting for result"); - } + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) +{ + if (!goal_handle) + { + RCLCPP_ERROR(this->get_logger(), "ledsPlay Goal was rejected by server"); + } + else + { + RCLCPP_INFO(this->get_logger(), "ledsPlay Goal accepted by server, waiting for result"); + } } void ChatActionServer::ledsPlayFeedbackCallback( rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback) { - + const std::shared_ptr feedback) +{ } void ChatActionServer::ledsPlayResultCallback( - const rclcpp_action::ClientGoalHandle::WrappedResult & result) { - switch (result.code) { + const rclcpp_action::ClientGoalHandle::WrappedResult& result) +{ + switch (result.code) + { case rclcpp_action::ResultCode::SUCCEEDED: - break; + break; case rclcpp_action::ResultCode::ABORTED: - RCLCPP_WARN(this->get_logger(), "ledsPlay Goal was aborted"); - return; + RCLCPP_WARN(this->get_logger(), "ledsPlay Goal was aborted"); + return; case rclcpp_action::ResultCode::CANCELED: - RCLCPP_WARN(this->get_logger(), "ledsPlay Goal was canceled"); - return; + RCLCPP_WARN(this->get_logger(), "ledsPlay Goal was canceled"); + return; default: - RCLCPP_ERROR(this->get_logger(), "ledsPlay Unknown result code"); - return; - } - - if (result.result->success) - RCLCPP_INFO(this->get_logger(), "Leds regulary played."); + RCLCPP_ERROR(this->get_logger(), "ledsPlay Unknown result code"); + return; + } + if (result.result->success) + RCLCPP_INFO(this->get_logger(), "Leds regulary played."); } -void ChatActionServer::eyesStatic( bool flag ) { - - using namespace std::placeholders; - if (!this->leds_play_act_client_->wait_for_action_server()) { - RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); - rclcpp::shutdown(); - } +void ChatActionServer::eyesStatic(bool flag) +{ + using namespace std::placeholders; + if (!this->leds_play_act_client_->wait_for_action_server()) + { + RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = nao_led_interfaces::action::LedsPlay::Goal(); + + goal_msg.leds = { nao_led_interfaces::msg::LedIndexes::REYE, nao_led_interfaces::msg::LedIndexes::LEYE }; + goal_msg.mode = nao_led_interfaces::msg::LedModes::STEADY; + std_msgs::msg::ColorRGBA color; + if (flag) + { + color.r = 1.0; + color.g = 1.0; + color.b = 1.0; + } + else + { + color.r = 0.0; + color.g = 0.0; + color.b = 0.0; + } + + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) + { + goal_msg.colors[i] = color; + } + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + send_goal_options.goal_response_callback = std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); + + // send_goal_options.feedback_callback = + // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); + + send_goal_options.result_callback = std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); + + auto goal_handle_future = leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); + RCLCPP_INFO(this->get_logger(), "ledsPlay eyesStatic goal sent: %d", flag); + + // synchronous + auto goal_handle = goal_handle_future.get(); + auto result_future = leds_play_act_client_->async_get_result(goal_handle); + auto result = result_future.get(); +} - auto goal_msg = nao_led_interfaces::action::LedsPlay::Goal(); +void ChatActionServer::chestStatic(bool flag) +{ + using namespace std::placeholders; + if (!this->leds_play_act_client_->wait_for_action_server()) + { + RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = nao_led_interfaces::action::LedsPlay::Goal(); + + goal_msg.leds = { nao_led_interfaces::msg::LedIndexes::CHEST }; + goal_msg.mode = nao_led_interfaces::msg::LedModes::STEADY; + std_msgs::msg::ColorRGBA color; + if (flag) + { + color.r = 1.0; + color.g = 1.0; + color.b = 1.0; + } + else + { + color.r = 0.0; + color.g = 0.0; + color.b = 0.0; + } + + goal_msg.colors[0] = color; + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + + send_goal_options.goal_response_callback = std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); + + // send_goal_options.feedback_callback = + // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); + + send_goal_options.result_callback = std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); + + auto goal_handle_future = this->leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); + RCLCPP_DEBUG(this->get_logger(), "ledsPlay chestStatic goal sent: %d", flag); + + // synchronous + auto goal_handle = goal_handle_future.get(); + auto result_future = leds_play_act_client_->async_get_result(goal_handle); + auto result = result_future.get(); +} - goal_msg.leds = {nao_led_interfaces::msg::LedIndexes::REYE, nao_led_interfaces::msg::LedIndexes::LEYE}; - goal_msg.mode = nao_led_interfaces::msg::LedModes::STEADY; - std_msgs::msg::ColorRGBA color; - if (flag) { - color.r = 1.0; color.g = 1.0; color.b = 1.0; - } else { - color.r = 0.0; color.g = 0.0; color.b = 0.0; +void ChatActionServer::headStatic(bool flag) +{ + using namespace std::placeholders; + + if (!this->leds_play_act_client_->wait_for_action_server()) + { + RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = nao_led_interfaces::action::LedsPlay::Goal(); + + goal_msg.leds = { nao_led_interfaces::msg::LedIndexes::HEAD }; + goal_msg.mode = nao_led_interfaces::msg::LedModes::STEADY; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) + { + if (flag) + { + goal_msg.intensities[i] = 1.0; } - - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) { - goal_msg.colors[i] = color; + else + { + goal_msg.intensities[i] = 0.0; } + } - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); - //send_goal_options.feedback_callback = - // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); + // send_goal_options.feedback_callback = + // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); + send_goal_options.result_callback = std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); - auto goal_handle_future = leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); - RCLCPP_INFO(this->get_logger(), "ledsPlay eyesStatic goal sent: %d", flag); + auto goal_handle_future = this->leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); + RCLCPP_DEBUG(this->get_logger(), "ledsPlay headStatic goal sent: "); - // synchronous - auto goal_handle = goal_handle_future.get(); - auto result_future = leds_play_act_client_->async_get_result(goal_handle); - auto result = result_future.get(); + // synchronous + auto goal_handle = goal_handle_future.get(); + auto result_future = leds_play_act_client_->async_get_result(goal_handle); + auto result = result_future.get(); } -void ChatActionServer::chestStatic( bool flag ) { - - using namespace std::placeholders; - if (!this->leds_play_act_client_->wait_for_action_server()) { - RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); - rclcpp::shutdown(); +void ChatActionServer::earsStatic(bool flag) +{ + using namespace std::placeholders; + + if (!this->leds_play_act_client_->wait_for_action_server()) + { + RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = nao_led_interfaces::action::LedsPlay::Goal(); + + goal_msg.leds = { nao_led_interfaces::msg::LedIndexes::REAR, nao_led_interfaces::msg::LedIndexes::LEAR }; + goal_msg.mode = nao_led_interfaces::msg::LedModes::STEADY; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) + { + if (flag) + { + goal_msg.intensities[i] = 1.0; } - - auto goal_msg = nao_led_interfaces::action::LedsPlay::Goal(); - - goal_msg.leds = {nao_led_interfaces::msg::LedIndexes::CHEST}; - goal_msg.mode = nao_led_interfaces::msg::LedModes::STEADY; - std_msgs::msg::ColorRGBA color; - if (flag) { - color.r = 1.0; color.g = 1.0; color.b = 1.0; - } else { - color.r = 0.0; color.g = 0.0; color.b = 0.0; + else + { + goal_msg.intensities[i] = 0.0; } + } - goal_msg.colors[0] = color; + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - send_goal_options.goal_response_callback = - std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); - //send_goal_options.feedback_callback = - // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); + // send_goal_options.feedback_callback = + // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); + send_goal_options.result_callback = std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); - auto goal_handle_future = this->leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); - RCLCPP_DEBUG(this->get_logger(), "ledsPlay chestStatic goal sent: %d", flag); - - // synchronous - auto goal_handle = goal_handle_future.get(); - auto result_future = leds_play_act_client_->async_get_result(goal_handle); - auto result = result_future.get(); + auto goal_handle_future = leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); + RCLCPP_DEBUG(this->get_logger(), "ledsPlay earsStatic goal sent: %d", flag); + // synchronous + auto goal_handle = goal_handle_future.get(); + auto result_future = leds_play_act_client_->async_get_result(goal_handle); + auto result = result_future.get(); } -void ChatActionServer::headStatic(bool flag) { - using namespace std::placeholders; +void ChatActionServer::headLoop(bool flag) +{ + using namespace std::placeholders; - if (!this->leds_play_act_client_->wait_for_action_server()) { - RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); - rclcpp::shutdown(); - } + if (!this->leds_play_act_client_->wait_for_action_server()) + { + RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); + rclcpp::shutdown(); + } + if (flag) + { auto goal_msg = nao_led_interfaces::action::LedsPlay::Goal(); - goal_msg.leds = {nao_led_interfaces::msg::LedIndexes::HEAD}; - goal_msg.mode = nao_led_interfaces::msg::LedModes::STEADY; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) { - if (flag) { - goal_msg.intensities[i] = 1.0; - } else {goal_msg.intensities[i] = 0.0;} + goal_msg.leds = { nao_led_interfaces::msg::LedIndexes::HEAD }; + goal_msg.mode = nao_led_interfaces::msg::LedModes::LOOP; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) + { + goal_msg.intensities[i] = 1.0; } + goal_msg.frequency = 10.0; auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); - //send_goal_options.feedback_callback = - // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); + // send_goal_options.feedback_callback = + // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); - - - auto goal_handle_future = this->leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); - RCLCPP_DEBUG(this->get_logger(), "ledsPlay headStatic goal sent: " ); - - // synchronous - auto goal_handle = goal_handle_future.get(); - auto result_future = leds_play_act_client_->async_get_result(goal_handle); - auto result = result_future.get(); + send_goal_options.result_callback = std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); + auto goal_handle_future = leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); + head_goal_handle_ = goal_handle_future.get(); + // head_goal_handle_ = client_ptr_->async_send_goal(goal_msg, send_goal_options); + RCLCPP_DEBUG(this->get_logger(), "headLoop: leds_play_act_client_ gets head_goal_handle_"); + } + else + { + // rclcpp_action::ClientGoalHandle::SharedPtr handle = + // head_goal_handle_.get(); auto cancel_result_future = client_ptr_->async_cancel_goal(handle); + auto cancel_result_future = leds_play_act_client_->async_cancel_goal(head_goal_handle_); + auto cancel_result = cancel_result_future.get(); // wait + RCLCPP_INFO(this->get_logger(), "headLoop: leds_play_act_client_ gets cancel_result"); + + // switch off the leds + this->headStatic(false); + } } -void ChatActionServer::earsStatic(bool flag) { - using namespace std::placeholders; +void ChatActionServer::earsLoop(bool flag) +{ + using namespace std::placeholders; - if (!this->leds_play_act_client_->wait_for_action_server()) { - RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); - rclcpp::shutdown(); + if (flag) + { + if (!this->leds_play_act_client_->wait_for_action_server()) + { + RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); + rclcpp::shutdown(); } - auto goal_msg = nao_led_interfaces::action::LedsPlay::Goal(); - goal_msg.leds = {nao_led_interfaces::msg::LedIndexes::REAR, nao_led_interfaces::msg::LedIndexes::LEAR}; - goal_msg.mode = nao_led_interfaces::msg::LedModes::STEADY; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) { - if (flag) { - goal_msg.intensities[i] = 1.0; - } else {goal_msg.intensities[i] = 0.0;} + goal_msg.leds = { nao_led_interfaces::msg::LedIndexes::REAR, nao_led_interfaces::msg::LedIndexes::LEAR }; + goal_msg.mode = nao_led_interfaces::msg::LedModes::LOOP; + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) + { + goal_msg.intensities[i] = 1.0; } + goal_msg.frequency = 10.0; auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); - //send_goal_options.feedback_callback = - // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); + // send_goal_options.feedback_callback = + // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); + send_goal_options.result_callback = std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); auto goal_handle_future = leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); - RCLCPP_DEBUG(this->get_logger(), "ledsPlay earsStatic goal sent: %d", flag); - - - // synchronous - auto goal_handle = goal_handle_future.get(); - auto result_future = leds_play_act_client_->async_get_result(goal_handle); - auto result = result_future.get(); - + ears_goal_handle_ = goal_handle_future.get(); + RCLCPP_INFO(this->get_logger(), "earsLoop: leds_play_act_client_ gets ears_goal_handle_"); + } + else + { + auto cancel_result_future = leds_play_act_client_->async_cancel_goal(ears_goal_handle_); + auto cancel_result = cancel_result_future.get(); // wait + RCLCPP_INFO(this->get_logger(), "earsLoop: leds_play_act_client_ gets cancel_result"); + + // switch off the leds + this->earsStatic(false); + } } -void ChatActionServer::headLoop( bool flag ) { - using namespace std::placeholders; +void ChatActionServer::eyesLoop(bool flag) +{ + using namespace std::placeholders; - if (!this->leds_play_act_client_->wait_for_action_server()) { - RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); - rclcpp::shutdown(); + if (flag) + { + if (!this->leds_play_act_client_->wait_for_action_server()) + { + RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); + rclcpp::shutdown(); } - if (flag) { - auto goal_msg = nao_led_interfaces::action::LedsPlay::Goal(); - - goal_msg.leds = {nao_led_interfaces::msg::LedIndexes::HEAD}; - goal_msg.mode = nao_led_interfaces::msg::LedModes::LOOP; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::HeadLeds::NUM_LEDS; ++i) { - goal_msg.intensities[i] = 1.0; - } - goal_msg.frequency = 10.0; - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - send_goal_options.goal_response_callback = - std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); - - // send_goal_options.feedback_callback = - // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); - - send_goal_options.result_callback = - std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); - - - auto goal_handle_future = leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); - head_goal_handle_ = goal_handle_future.get(); - //head_goal_handle_ = client_ptr_->async_send_goal(goal_msg, send_goal_options); - RCLCPP_DEBUG(this->get_logger(), "headLoop: leds_play_act_client_ gets head_goal_handle_" ); - } else { - //rclcpp_action::ClientGoalHandle::SharedPtr handle = head_goal_handle_.get(); - //auto cancel_result_future = client_ptr_->async_cancel_goal(handle); - auto cancel_result_future = leds_play_act_client_->async_cancel_goal(head_goal_handle_); - auto cancel_result = cancel_result_future.get(); //wait - RCLCPP_INFO(this->get_logger(), "headLoop: leds_play_act_client_ gets cancel_result"); - - // switch off the leds - this->headStatic(false); - } -} - -void ChatActionServer::earsLoop(bool flag) { - using namespace std::placeholders; - - if (flag) { - if (!this->leds_play_act_client_->wait_for_action_server()) { - RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); - rclcpp::shutdown(); - } - - auto goal_msg = nao_led_interfaces::action::LedsPlay::Goal(); - - goal_msg.leds = {nao_led_interfaces::msg::LedIndexes::REAR, nao_led_interfaces::msg::LedIndexes::LEAR}; - goal_msg.mode = nao_led_interfaces::msg::LedModes::LOOP; - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEarLeds::NUM_LEDS; ++i) { - goal_msg.intensities[i] = 1.0; - } - goal_msg.frequency = 10.0; - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - send_goal_options.goal_response_callback = - std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); - - //send_goal_options.feedback_callback = - // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); - - send_goal_options.result_callback = - std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); + auto goal_msg = nao_led_interfaces::action::LedsPlay::Goal(); - auto goal_handle_future = leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); - ears_goal_handle_ = goal_handle_future.get(); - RCLCPP_INFO(this->get_logger(), "earsLoop: leds_play_act_client_ gets ears_goal_handle_" ); - } else { - auto cancel_result_future = leds_play_act_client_->async_cancel_goal(ears_goal_handle_); - auto cancel_result = cancel_result_future.get(); //wait - RCLCPP_INFO(this->get_logger(), "earsLoop: leds_play_act_client_ gets cancel_result"); + goal_msg.leds = { nao_led_interfaces::msg::LedIndexes::REYE, nao_led_interfaces::msg::LedIndexes::LEYE }; + goal_msg.mode = nao_led_interfaces::msg::LedModes::LOOP; + std_msgs::msg::ColorRGBA color; + color.r = 1.0; + color.g = 1.0; + color.b = 1.0; - // switch off the leds - this->earsStatic(false); + for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) + { + goal_msg.colors[i] = color; } -} - - -void ChatActionServer::eyesLoop(bool flag) { - using namespace std::placeholders; - - if (flag) { - if (!this->leds_play_act_client_->wait_for_action_server()) { - RCLCPP_ERROR(this->get_logger(), "ledsPlay Action server not available after waiting"); - rclcpp::shutdown(); - } - - auto goal_msg = nao_led_interfaces::action::LedsPlay::Goal(); - - goal_msg.leds = {nao_led_interfaces::msg::LedIndexes::REYE, nao_led_interfaces::msg::LedIndexes::LEYE}; - goal_msg.mode = nao_led_interfaces::msg::LedModes::LOOP; - std_msgs::msg::ColorRGBA color; - color.r = 1.0; color.g = 1.0; color.b = 1.0; - - for (unsigned i = 0; i < nao_lola_command_msgs::msg::RightEyeLeds::NUM_LEDS; ++i) { - goal_msg.colors[i] = color; - } - goal_msg.frequency = 5.0; + goal_msg.frequency = 5.0; - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - - send_goal_options.goal_response_callback = - std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - //send_goal_options.feedback_callback = - // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); + send_goal_options.goal_response_callback = std::bind(&ChatActionServer::ledsPlayGoalResponseCallback, this, _1); - send_goal_options.result_callback = - std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); + // send_goal_options.feedback_callback = + // std::bind(&ChatActionServer::ledsPlayFeedbackCallback, this, _1, _2); - RCLCPP_INFO(this->get_logger(), "Sending goal:" ); + send_goal_options.result_callback = std::bind(&ChatActionServer::ledsPlayResultCallback, this, _1); - auto goal_handle_future = leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); - eyes_goal_handle_ = goal_handle_future.get(); - RCLCPP_INFO(this->get_logger(), "eyesLoop: leds_play_act_client_ gets ears_goal_handle_" ); - } else { - auto cancel_result_future = leds_play_act_client_->async_cancel_goal(eyes_goal_handle_); - auto cancel_result = cancel_result_future.get(); //wait - RCLCPP_INFO(this->get_logger(), "eyesLoop: leds_play_act_client_ gets cancel_result"); + RCLCPP_INFO(this->get_logger(), "Sending goal:"); - // switch off the leds - this->eyesStatic(false); - } + auto goal_handle_future = leds_play_act_client_->async_send_goal(goal_msg, send_goal_options); + eyes_goal_handle_ = goal_handle_future.get(); + RCLCPP_INFO(this->get_logger(), "eyesLoop: leds_play_act_client_ gets ears_goal_handle_"); + } + else + { + auto cancel_result_future = leds_play_act_client_->async_cancel_goal(eyes_goal_handle_); + auto cancel_result = cancel_result_future.get(); // wait + RCLCPP_INFO(this->get_logger(), "eyesLoop: leds_play_act_client_ gets cancel_result"); + + // switch off the leds + this->eyesStatic(false); + } } - /*############## JOINTS PLAY ACTION CLIENT ##############*/ void ChatActionServer::jointsPlayGoalResponseCallback( - const rclcpp_action::ClientGoalHandle::SharedPtr & goal_handle) { - if (!goal_handle) { - RCLCPP_ERROR(this->get_logger(), "jointsPlay goal was rejected by server"); - } else { - RCLCPP_INFO(this->get_logger(), "jointsPlay goal accepted by server, waiting for result"); - } + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) +{ + if (!goal_handle) + { + RCLCPP_ERROR(this->get_logger(), "jointsPlay goal was rejected by server"); + } + else + { + RCLCPP_INFO(this->get_logger(), "jointsPlay goal accepted by server, waiting for result"); + } } void ChatActionServer::jointsPlayFeedbackCallback( rclcpp_action::ClientGoalHandle::SharedPtr, - const std::shared_ptr feedback) { - - //TODO - + const std::shared_ptr feedback) +{ + // TODO } void ChatActionServer::jointsPlayResultCallback( - const rclcpp_action::ClientGoalHandle::WrappedResult & result) { - - switch (result.code) { + const rclcpp_action::ClientGoalHandle::WrappedResult& result) +{ + switch (result.code) + { case rclcpp_action::ResultCode::SUCCEEDED: - break; + break; case rclcpp_action::ResultCode::ABORTED: - RCLCPP_WARN(this->get_logger(), " jointsPlay Goal was aborted"); - return; + RCLCPP_WARN(this->get_logger(), " jointsPlay Goal was aborted"); + return; case rclcpp_action::ResultCode::CANCELED: - RCLCPP_WARN(this->get_logger(), "jointsPlay Goal was canceled"); - return; + RCLCPP_WARN(this->get_logger(), "jointsPlay Goal was canceled"); + return; default: - RCLCPP_ERROR(this->get_logger(), "jointsPlay Unknown result code"); - return; - } - - if (result.result->success) - RCLCPP_INFO(this->get_logger(), "Joints posisitions regulary played."); + RCLCPP_ERROR(this->get_logger(), "jointsPlay Unknown result code"); + return; + } + if (result.result->success) + RCLCPP_INFO(this->get_logger(), "Joints posisitions regulary played."); } - } // namespace hni_chat_action_server RCLCPP_COMPONENTS_REGISTER_NODE(hni_chat_action_server::ChatActionServer) \ No newline at end of file diff --git a/hni_cpp/src/chat_service_client.cpp b/hni_cpp/src/chat_service_client.cpp index a15394f..4ee251a 100644 --- a/hni_cpp/src/chat_service_client.cpp +++ b/hni_cpp/src/chat_service_client.cpp @@ -14,7 +14,6 @@ #include - #include "ament_index_cpp/get_package_share_directory.hpp" #include "rclcpp/rclcpp.hpp" @@ -22,26 +21,29 @@ #include "hni_cpp/chat_service_client.hpp" +namespace hni_chat_service_client +{ -namespace hni_chat_service_client { - -ChatServiceClient::ChatServiceClient(const rclcpp::NodeOptions & options) - : rclcpp::Node("chat_service_client_node", options) { - - this->client_ptr_ = this->create_client("chatGPT_service"); +ChatServiceClient::ChatServiceClient(const rclcpp::NodeOptions& options) + : rclcpp::Node("chat_service_client_node", options) +{ + this->client_ptr_ = this->create_client("chatGPT_service"); RCLCPP_INFO(this->get_logger(), "ChatServiceClient initialized"); - } -ChatServiceClient::~ChatServiceClient() {} - -std::string ChatServiceClient::sendSyncReq(std::string & phrase) { +ChatServiceClient::~ChatServiceClient() +{ +} +std::string ChatServiceClient::sendSyncReq(std::string& phrase) +{ using namespace std::chrono_literals; - while (!client_ptr_->wait_for_service(1s)) { - if (!rclcpp::ok()) { + while (!client_ptr_->wait_for_service(1s)) + { + if (!rclcpp::ok()) + { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for chat service. Exiting."); return "ERROR"; } @@ -57,16 +59,18 @@ std::string ChatServiceClient::sendSyncReq(std::string & phrase) { RCLCPP_DEBUG(this->get_logger(), "wait chatGPT answer.."); // Wait for the result. if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), chat_result) == - rclcpp::FutureReturnCode::SUCCESS) { + rclcpp::FutureReturnCode::SUCCESS) + { chatgpt_answer = chat_result.get()->answer; RCLCPP_INFO(this->get_logger(), ("chatGPT answer: " + chatgpt_answer).c_str()); - } else { + } + else + { RCLCPP_ERROR(this->get_logger(), "Failed to call chat_service"); return "ERROR"; } RCLCPP_DEBUG(this->get_logger(), "chat request completed"); return chatgpt_answer; - } } // namespace hni_chat_service_client diff --git a/hni_cpp/src/gstt_service_client.cpp b/hni_cpp/src/gstt_service_client.cpp index 038f153..096302a 100644 --- a/hni_cpp/src/gstt_service_client.cpp +++ b/hni_cpp/src/gstt_service_client.cpp @@ -27,27 +27,29 @@ #include "hni_cpp/gstt_service_client.hpp" #include "std_srvs/srv/set_bool.hpp" +namespace hni_gstt_service_client +{ -namespace hni_gstt_service_client { - - -GsttServiceClient::GsttServiceClient(const rclcpp::NodeOptions & options) - : rclcpp::Node("gstt_service_client_node", options) { - - this->client_ptr_ = this->create_client("gstt_service"); +GsttServiceClient::GsttServiceClient(const rclcpp::NodeOptions& options) + : rclcpp::Node("gstt_service_client_node", options) +{ + this->client_ptr_ = this->create_client("gstt_service"); RCLCPP_INFO(this->get_logger(), "GsttServiceClient initialized"); - } -GsttServiceClient::~GsttServiceClient() {} - +GsttServiceClient::~GsttServiceClient() +{ +} -std::string GsttServiceClient::sendSyncReq() { +std::string GsttServiceClient::sendSyncReq() +{ using namespace std::chrono_literals; - while (!client_ptr_->wait_for_service(1s)) { - if (!rclcpp::ok()) { + while (!client_ptr_->wait_for_service(1s)) + { + if (!rclcpp::ok()) + { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for GSTT service. Exiting."); return "ERROR"; } @@ -60,19 +62,21 @@ std::string GsttServiceClient::sendSyncReq() { RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "GSTT sending request..."); auto gstt_result = client_ptr_->async_send_request(gstt_request); // Wait for the result. - if ( rclcpp::spin_until_future_complete(this->get_node_base_interface(), gstt_result) == - rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), gstt_result) == + rclcpp::FutureReturnCode::SUCCESS) + { RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "inside spin_until_future_complete"); - //if ( rclcpp::spin_until_future_complete(this, gstt_result) == rclcpp::FutureReturnCode::SUCCESS ) { + // if ( rclcpp::spin_until_future_complete(this, gstt_result) == rclcpp::FutureReturnCode::SUCCESS ) { recognized_speach = gstt_result.get()->message; RCLCPP_INFO(this->get_logger(), ("Recognized speach: " + recognized_speach).c_str()); - } else { + } + else + { RCLCPP_ERROR(this->get_logger(), "Failed to call gstt_service"); return "ERROR"; } return recognized_speach; - } } // namespace hni_gstt_service_client diff --git a/hni_cpp/src/gtts_service_client.cpp b/hni_cpp/src/gtts_service_client.cpp index 78a6747..485c3ac 100644 --- a/hni_cpp/src/gtts_service_client.cpp +++ b/hni_cpp/src/gtts_service_client.cpp @@ -26,26 +26,29 @@ #include "hni_cpp/gtts_service_client.hpp" +namespace hni_gtts_service_client +{ -namespace hni_gtts_service_client { - -GttsServiceClient::GttsServiceClient(const rclcpp::NodeOptions & options) - : rclcpp::Node("gtts_service_client_node", options) { - - this->client_ptr_ = this->create_client("gtts_service"); +GttsServiceClient::GttsServiceClient(const rclcpp::NodeOptions& options) + : rclcpp::Node("gtts_service_client_node", options) +{ + this->client_ptr_ = this->create_client("gtts_service"); RCLCPP_INFO(this->get_logger(), "GttsServiceClient initialized"); - } -GttsServiceClient::~GttsServiceClient() {} - -void GttsServiceClient::sendSyncReq(std::string & text_to_speak) { +GttsServiceClient::~GttsServiceClient() +{ +} +void GttsServiceClient::sendSyncReq(std::string& text_to_speak) +{ using namespace std::chrono_literals; - while (!client_ptr_->wait_for_service(1s)) { - if (!rclcpp::ok()) { + while (!client_ptr_->wait_for_service(1s)) + { + if (!rclcpp::ok()) + { RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for GTTS service. Exiting."); return; } @@ -58,9 +61,12 @@ void GttsServiceClient::sendSyncReq(std::string & text_to_speak) { auto gtts_result = client_ptr_->async_send_request(gtts_request); // Wait for the result. if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), gtts_result) == - rclcpp::FutureReturnCode::SUCCESS) { + rclcpp::FutureReturnCode::SUCCESS) + { RCLCPP_INFO(this->get_logger(), "GTTS request completed: %d", gtts_result.get()->success); - } else { + } + else + { RCLCPP_ERROR(this->get_logger(), "Failed to call gtts_service"); return; } diff --git a/hni_cpp/src/head_track_action_client.cpp b/hni_cpp/src/head_track_action_client.cpp index b7894bc..b472feb 100644 --- a/hni_cpp/src/head_track_action_client.cpp +++ b/hni_cpp/src/head_track_action_client.cpp @@ -15,41 +15,37 @@ #include "nao_lola_command_msgs/msg/joint_indexes.hpp" #include "nao_lola_command_msgs/msg/joint_stiffnesses.hpp" - namespace fs = boost::filesystem; +namespace hni_head_track_cpp +{ -namespace hni_head_track_cpp { - -class HeadTrackActionClient : public rclcpp::Node { - public: - +class HeadTrackActionClient : public rclcpp::Node +{ +public: using ObjTrack = hni_interfaces::action::VideoTracker; using GoalHandleObjTrack = rclcpp_action::ClientGoalHandle; - explicit HeadTrackActionClient(const rclcpp::NodeOptions & options) - : Node("head_track_action_client", options) { - - this->client_ptr_ = rclcpp_action::create_client( - this, - "head_track"); + explicit HeadTrackActionClient(const rclcpp::NodeOptions& options) : Node("head_track_action_client", options) + { + this->client_ptr_ = rclcpp_action::create_client(this, "head_track"); - this->timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&HeadTrackActionClient::send_goal, this)); + this->timer_ = + this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&HeadTrackActionClient::send_goal, this)); - //this->declare_parameter("file", getDefaultFullFilePath()); + // this->declare_parameter("file", getDefaultFullFilePath()); RCLCPP_INFO(this->get_logger(), "HeadTrackActionClient initialized"); - } - void send_goal() { + void send_goal() + { using namespace std::placeholders; this->timer_->cancel(); - if (!this->client_ptr_->wait_for_action_server()) { + if (!this->client_ptr_->wait_for_action_server()) + { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } @@ -58,31 +54,27 @@ class HeadTrackActionClient : public rclcpp::Node { goal_msg.camera_id = " "; - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&HeadTrackActionClient::goal_response_callback, this, _1); - - //send_goal_options.feedback_callback = - // std::bind(&HeadTrackActionClient::feedback_callback, this, _1, _2); - - send_goal_options.result_callback = - std::bind(&HeadTrackActionClient::result_callback, this, _1); + send_goal_options.goal_response_callback = std::bind(&HeadTrackActionClient::goal_response_callback, this, _1); - RCLCPP_INFO(this->get_logger(), "Sending goal: " ); + // send_goal_options.feedback_callback = + // std::bind(&HeadTrackActionClient::feedback_callback, this, _1, _2); - this->client_ptr_->async_send_goal(goal_msg, send_goal_options); - } + send_goal_options.result_callback = std::bind(&HeadTrackActionClient::result_callback, this, _1); + RCLCPP_INFO(this->get_logger(), "Sending goal: "); - private: + this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + } +private: rclcpp_action::Client::SharedPtr client_ptr_; rclcpp::TimerBase::SharedPtr timer_; - std::string getDefaultFullFilePath() { - + std::string getDefaultFullFilePath() + { /* RCLCPP_INFO(this->get_logger(),"getDefaultFullFilePath "); std::string file = "moves/hello.txt"; @@ -100,41 +92,43 @@ class HeadTrackActionClient : public rclcpp::Node { RCLCPP_INFO(this->get_logger(), ("File path " + full_path.string()).c_str()); return full_path.string(); */ - + return "/home/nao/rolling_ws/src/hni/hni_moves/moves/fear.txt"; - //return "hello.txt"; //WORKING + // return "hello.txt"; //WORKING } - - void goal_response_callback(const GoalHandleObjTrack::SharedPtr & goal_handle) { - if (!goal_handle) { + void goal_response_callback(const GoalHandleObjTrack::SharedPtr& goal_handle) + { + if (!goal_handle) + { RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } else { + } + else + { RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); } } - void feedback_callback( - GoalHandleObjTrack::SharedPtr, - const std::shared_ptr feedback) { - - //TODO - + void feedback_callback(GoalHandleObjTrack::SharedPtr, const std::shared_ptr feedback) + { + // TODO } - void result_callback(const GoalHandleObjTrack::WrappedResult & result) { - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; + void result_callback(const GoalHandleObjTrack::WrappedResult& result) + { + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; } if (result.result->success) diff --git a/hni_cpp/src/head_track_action_server.cpp b/hni_cpp/src/head_track_action_server.cpp index 1478a0f..67e47be 100644 --- a/hni_cpp/src/head_track_action_server.cpp +++ b/hni_cpp/src/head_track_action_server.cpp @@ -19,9 +19,9 @@ #include #include -#include // std::string, std::stof -#include // std::cout -#include // std::queue +#include // std::string, std::stof +#include // std::cout +#include // std::queue #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -37,62 +37,60 @@ #include "hni_cpp/head_track_action_server.hpp" - -namespace hni_head_track_action_server { - -HeadTrackActionServer::HeadTrackActionServer(const rclcpp::NodeOptions & options) - : rclcpp::Node("head_track_action_server_node", options), - kSecToHeadReset_(6), kTrackMaxSize_(2), kHeadWidthStep_(0.36), kHeadHeightStep_(0.25), kVerResolution_(480), kHorResolution_(640) { - +namespace hni_head_track_action_server +{ + +HeadTrackActionServer::HeadTrackActionServer(const rclcpp::NodeOptions& options) + : rclcpp::Node("head_track_action_server_node", options) + , kSecToHeadReset_(6) + , kTrackMaxSize_(2) + , kHeadWidthStep_(0.36) + , kHeadHeightStep_(0.25) + , kVerResolution_(480) + , kHorResolution_(640) +{ using namespace std::placeholders; - this->client_ptr_ = rclcpp_action::create_client( - this, - "face_tracker"); + this->client_ptr_ = rclcpp_action::create_client(this, "face_tracker"); - this->timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&HeadTrackActionServer::sendGoal, this)); + this->timer_ = + this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&HeadTrackActionServer::sendGoal, this)); this->jpos_sub_ = this->create_subscription( - "sensors/joint_positions", 10, std::bind(&HeadTrackActionServer::jposCallback, this, _1)); - + "sensors/joint_positions", 10, std::bind(&HeadTrackActionServer::jposCallback, this, _1)); - this->jpos_pub_ = this->create_publisher( - "effectors/joint_positions", 10); - - this->jstiff_pub_ = this->create_publisher( - "effectors/joint_stiffnesses", 10); + this->jpos_pub_ = this->create_publisher("effectors/joint_positions", 10); + this->jstiff_pub_ = + this->create_publisher("effectors/joint_stiffnesses", 10); this->action_server_ = rclcpp_action::create_server( - this, - "head_track", - std::bind(&HeadTrackActionServer::handleGoal, this, _1, _2), - std::bind(&HeadTrackActionServer::handleCancel, this, _1), - std::bind(&HeadTrackActionServer::handleAccepted, this, _1)); + this, "head_track", std::bind(&HeadTrackActionServer::handleGoal, this, _1, _2), + std::bind(&HeadTrackActionServer::handleCancel, this, _1), + std::bind(&HeadTrackActionServer::handleAccepted, this, _1)); RCLCPP_INFO(this->get_logger(), "HeadTrackActionServer Initialized"); - //this->play_cmd_sub_ = this->create_subscription( - // "face_pos_topic", 10, std::bind(&HeadTrackActionServer::face_pos_callback, this, _1)); - - //this->obj_pos_sub_ = this->create_subscription( - // "face_tracker/_action/feedback", 10, std::bind(&HeadTrackActionServer::obj_pos_callback, this, _1)); + // this->play_cmd_sub_ = this->create_subscription( + // "face_pos_topic", 10, std::bind(&HeadTrackActionServer::face_pos_callback, this, _1)); + // this->obj_pos_sub_ = this->create_subscription( + // "face_tracker/_action/feedback", 10, std::bind(&HeadTrackActionServer::obj_pos_callback, + // this, _1)); } -HeadTrackActionServer::~HeadTrackActionServer() {} +HeadTrackActionServer::~HeadTrackActionServer() +{ +} nao_lola_command_msgs::msg::JointIndexes joint_indexes_msg_; -uint8_t head_joint_indexes_ [2] = { joint_indexes_msg_.HEADYAW, - joint_indexes_msg_.HEADPITCH, - }; +uint8_t head_joint_indexes_[2] = { + joint_indexes_msg_.HEADYAW, + joint_indexes_msg_.HEADPITCH, +}; uint8_t num_rec_joints_ = sizeof(head_joint_indexes_) / sizeof(head_joint_indexes_[0]); - - -//bool fileSuccessfullyRead_ = false; +// bool fileSuccessfullyRead_ = false; // ###################################### functions ################################ @@ -111,23 +109,24 @@ uint8_t num_rec_joints_ = sizeof(head_joint_indexes_) / sizeof(head_joint_indexe } */ -void HeadTrackActionServer::jposCallback(const nao_lola_sensor_msgs::msg::JointPositions & joints) { - +void HeadTrackActionServer::jposCallback(const nao_lola_sensor_msgs::msg::JointPositions& joints) +{ last_yaw_ = joints.positions[joint_indexes_msg_.HEADYAW]; last_pitch_ = joints.positions[joint_indexes_msg_.HEADPITCH]; RCLCPP_DEBUG_STREAM(this->get_logger(), "New joint positions saved"); } - // ###################################### client ################################ -void HeadTrackActionServer::sendGoal() { +void HeadTrackActionServer::sendGoal() +{ using namespace std::placeholders; this->timer_->cancel(); - if (!this->client_ptr_->wait_for_action_server()) { + if (!this->client_ptr_->wait_for_action_server()) + { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } @@ -138,58 +137,58 @@ void HeadTrackActionServer::sendGoal() { auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&HeadTrackActionServer::goalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&HeadTrackActionServer::goalResponseCallback, this, _1); - send_goal_options.feedback_callback = - std::bind(&HeadTrackActionServer::feedbackCallback, this, _1, _2); + send_goal_options.feedback_callback = std::bind(&HeadTrackActionServer::feedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&HeadTrackActionServer::resultCallback, this, _1); + send_goal_options.result_callback = std::bind(&HeadTrackActionServer::resultCallback, this, _1); - RCLCPP_INFO(this->get_logger(), "Sending obj tracking goal " ); + RCLCPP_INFO(this->get_logger(), "Sending obj tracking goal "); this->client_ptr_->async_send_goal(goal_msg, send_goal_options); } - -void HeadTrackActionServer::goalResponseCallback( - const GoalHandleObjTrack::SharedPtr & goal_handle) { - if (!goal_handle) { +void HeadTrackActionServer::goalResponseCallback(const GoalHandleObjTrack::SharedPtr& goal_handle) +{ + if (!goal_handle) + { RCLCPP_ERROR(this->get_logger(), "Goal was rejected by object tracking server"); - } else { + } + else + { RCLCPP_INFO(this->get_logger(), "Goal accepted by object tracking server, waiting for result"); } } -void HeadTrackActionServer::feedbackCallback( - GoalHandleObjTrack::SharedPtr, - const std::shared_ptr feedback) { +void HeadTrackActionServer::feedbackCallback(GoalHandleObjTrack::SharedPtr, + const std::shared_ptr feedback) +{ + x_track_.push(feedback->center.x); // from 0 to kHorResolution_ - x_track_.push(feedback->center.x); //from 0 to kHorResolution_ + y_track_.push(feedback->center.y); // from 0 to kVerResolution_ - y_track_.push(feedback->center.y); //from 0 to kVerResolution_ - - if (x_track_.size() > kTrackMaxSize_) { + if (x_track_.size() > kTrackMaxSize_) + { x_track_.pop(); y_track_.pop(); } - } -void HeadTrackActionServer::resultCallback(const GoalHandleObjTrack::WrappedResult & result) { - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; +void HeadTrackActionServer::resultCallback(const GoalHandleObjTrack::WrappedResult& result) +{ + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; } if (result.result->success) @@ -198,20 +197,18 @@ void HeadTrackActionServer::resultCallback(const GoalHandleObjTrack::WrappedResu rclcpp::shutdown(); } - // ###################################### server ################################ - -rclcpp_action::GoalResponse HeadTrackActionServer::handleGoal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) { - - RCLCPP_INFO( this->get_logger(), "Received goal request" ); +rclcpp_action::GoalResponse HeadTrackActionServer::handleGoal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) +{ + RCLCPP_INFO(this->get_logger(), "Received goal request"); (void)uuid; - if (true) { - - for (auto i : head_joint_indexes_) { + if (true) + { + for (auto i : head_joint_indexes_) + { jstiff_cmd_.indexes.push_back(i); jstiff_cmd_.stiffnesses.push_back(1.0); } @@ -221,42 +218,43 @@ rclcpp_action::GoalResponse HeadTrackActionServer::handleGoal( RCLCPP_DEBUG_STREAM(this->get_logger(), "Publishing 1.0 on effectors/joint_stiffnesses"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - - } else { - - //RCLCPP_ERROR(this->get_logger(), ("Couldn't open file " + goal->path).c_str() ); - //fileSuccessfullyRead_ = false; + } + else + { + // RCLCPP_ERROR(this->get_logger(), ("Couldn't open file " + goal->path).c_str() ); + // fileSuccessfullyRead_ = false; return rclcpp_action::GoalResponse::REJECT; } - } -rclcpp_action::CancelResponse HeadTrackActionServer::handleCancel( - const std::shared_ptr goal_handle) { +rclcpp_action::CancelResponse +HeadTrackActionServer::handleCancel(const std::shared_ptr goal_handle) +{ RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; } -void HeadTrackActionServer::handleAccepted(const std::shared_ptr goal_handle) { +void HeadTrackActionServer::handleAccepted(const std::shared_ptr goal_handle) +{ using namespace std::placeholders; // this needs to return quickly to avoid blocking the executor, so spin up a new thread - std::thread{std::bind(&HeadTrackActionServer::execute, this, _1), goal_handle} .detach(); + std::thread{ std::bind(&HeadTrackActionServer::execute, this, _1), goal_handle }.detach(); } -void HeadTrackActionServer::execute(const std::shared_ptr goal_handle) { - +void HeadTrackActionServer::execute(const std::shared_ptr goal_handle) +{ RCLCPP_INFO(this->get_logger(), "Executing goal"); const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared(); - //auto percentage = feedback->center; + // auto percentage = feedback->center; auto result = std::make_shared(); float x; float y; - //float cur_pitch = 0.0; - //float cur_yaw = 0.0; + // float cur_pitch = 0.0; + // float cur_yaw = 0.0; bool both = false; bool tracking = false; bool right_available, left_available; @@ -264,14 +262,14 @@ void HeadTrackActionServer::execute(const std::shared_ptr g bool first_miss_face = true, reset_head = false; rclcpp::Time noFaceTime; + rclcpp::Rate loop_rate(0.5); // Hz - rclcpp::Rate loop_rate(0.5); //Hz - - - while (rclcpp::ok()) { + while (rclcpp::ok()) + { RCLCPP_INFO(this->get_logger(), "inside while"); // Check if there is a cancel request - if (goal_handle->is_canceling()) { + if (goal_handle->is_canceling()) + { result->success = false; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Goal canceled"); @@ -282,131 +280,157 @@ void HeadTrackActionServer::execute(const std::shared_ptr g tracking = false; right_available = last_yaw_ >= -3 * kHeadWidthStep_; - left_available = last_yaw_ <= 3 * kHeadWidthStep_; - up_available = last_pitch_ >= -2 * kHeadHeightStep_; - down_available = last_pitch_ <= 2 * kHeadHeightStep_; + left_available = last_yaw_ <= 3 * kHeadWidthStep_; + up_available = last_pitch_ >= -2 * kHeadHeightStep_; + down_available = last_pitch_ <= 2 * kHeadHeightStep_; x = x_track_.back(); y = y_track_.back(); - RCLCPP_INFO(this->get_logger(), ("Received point (x,y) : (" + std::to_string(x) + ", " + std::to_string(y) + ")" ).c_str() ); + RCLCPP_INFO(this->get_logger(), + ("Received point (x,y) : (" + std::to_string(x) + ", " + std::to_string(y) + ")").c_str()); - if ( x != -1 && y != -1) { // face detected + if (x != -1 && y != -1) + { // face detected RCLCPP_INFO(this->get_logger(), "face detected"); - if (x < 0.25 * kHorResolution_ ) { - - if (y < 0.25 * kVerResolution_ ) { + if (x < 0.25 * kHorResolution_) + { + if (y < 0.25 * kVerResolution_) + { // upper left RCLCPP_INFO(this->get_logger(), "upper left"); both = true; - if (left_available) { + if (left_available) + { tracking = true; jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADYAW); jpos_cmd_.positions.push_back(last_yaw_ + 1 * kHeadWidthStep_); } - if (up_available) { + if (up_available) + { tracking = true; jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADPITCH); jpos_cmd_.positions.push_back(last_pitch_ - 1 * kHeadHeightStep_); } - - } else if (y > 0.75 * kVerResolution_) { + } + else if (y > 0.75 * kVerResolution_) + { // down left RCLCPP_INFO(this->get_logger(), "down left"); both = true; - if (left_available <= 2 * kHeadWidthStep_) { + if (left_available <= 2 * kHeadWidthStep_) + { tracking = true; jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADYAW); jpos_cmd_.positions.push_back(last_yaw_ + 1 * kHeadWidthStep_); } - if (down_available) { + if (down_available) + { tracking = true; jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADPITCH); jpos_cmd_.positions.push_back(last_pitch_ + 1 * kHeadHeightStep_); } - - } else { + } + else + { // left RCLCPP_INFO(this->get_logger(), "left"); - if (left_available) { + if (left_available) + { tracking = true; jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADYAW); jpos_cmd_.positions.push_back(last_yaw_ + 1 * kHeadWidthStep_); } } - - } else if (x > 0.75 * kHorResolution_) { - if (y < 0.25 * kVerResolution_ ) { + } + else if (x > 0.75 * kHorResolution_) + { + if (y < 0.25 * kVerResolution_) + { // upper right RCLCPP_INFO(this->get_logger(), "upper right"); both = true; - if (right_available) { + if (right_available) + { tracking = true; jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADYAW); jpos_cmd_.positions.push_back(last_yaw_ - 1 * kHeadWidthStep_); } - if (up_available) { + if (up_available) + { tracking = true; jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADPITCH); jpos_cmd_.positions.push_back(last_pitch_ - 1 * kHeadHeightStep_); } - - } else if (y > 0.75 * kVerResolution_) { - //down right + } + else if (y > 0.75 * kVerResolution_) + { + // down right RCLCPP_INFO(this->get_logger(), "down right"); both = true; - if (right_available) { + if (right_available) + { tracking = true; jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADYAW); jpos_cmd_.positions.push_back(last_yaw_ - 1 * kHeadWidthStep_); } - if (down_available) { + if (down_available) + { tracking = true; jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADPITCH); jpos_cmd_.positions.push_back(last_pitch_ + 1 * kHeadHeightStep_); } - - } else { + } + else + { // right RCLCPP_INFO(this->get_logger(), "right"); - if (right_available) { + if (right_available) + { tracking = true; jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADYAW); jpos_cmd_.positions.push_back(last_yaw_ - 1 * kHeadWidthStep_); } } - } else if ( !both && y < 0.25 * kVerResolution_ ) { - //up + } + else if (!both && y < 0.25 * kVerResolution_) + { + // up RCLCPP_INFO(this->get_logger(), "up"); - if (up_available) { + if (up_available) + { tracking = true; jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADPITCH); jpos_cmd_.positions.push_back(last_pitch_ - 1 * kHeadHeightStep_); } - - } else if ( !both && y > 0.75 * kVerResolution_ ) { - //down + } + else if (!both && y > 0.75 * kVerResolution_) + { + // down RCLCPP_INFO(this->get_logger(), "down"); - if (down_available) { + if (down_available) + { tracking = true; jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADPITCH); jpos_cmd_.positions.push_back(last_pitch_ + 1 * kHeadHeightStep_); } - } else { - //center of the frame + } + else + { + // center of the frame RCLCPP_INFO(this->get_logger(), "center"); - } // send command - if (tracking) { + if (tracking) + { jpos_pub_->publish(jpos_cmd_); RCLCPP_INFO(this->get_logger(), "Publishing on effectors/joint_positions"); jpos_cmd_.indexes.clear(); @@ -415,15 +439,19 @@ void HeadTrackActionServer::execute(const std::shared_ptr g reset_head = false; } // object position available - } else { + } + else + { RCLCPP_INFO(this->get_logger(), "No face detected"); - if (first_miss_face) { + if (first_miss_face) + { first_miss_face = false; - noFaceTime = rclcpp::Clock{RCL_ROS_TIME} .now(); + noFaceTime = rclcpp::Clock{ RCL_ROS_TIME }.now(); RCLCPP_INFO_STREAM(this->get_logger(), "first no face time: " << noFaceTime.seconds()); - } else if (rclcpp::Clock{RCL_ROS_TIME} .now().seconds() - noFaceTime.seconds() > kSecToHeadReset_ - && !reset_head) { + } + else if (rclcpp::Clock{ RCL_ROS_TIME }.now().seconds() - noFaceTime.seconds() > kSecToHeadReset_ && !reset_head) + { jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADPITCH); jpos_cmd_.positions.push_back(0.0); jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADYAW); @@ -434,10 +462,9 @@ void HeadTrackActionServer::execute(const std::shared_ptr g } loop_rate.sleep(); - } } -} // namespace hni_head_track_action_server +} // namespace hni_head_track_action_server RCLCPP_COMPONENTS_REGISTER_NODE(hni_head_track_action_server::HeadTrackActionServer) \ No newline at end of file diff --git a/hni_cpp/src/head_track_action_server2.cpp b/hni_cpp/src/head_track_action_server2.cpp index f237c0d..662fe69 100644 --- a/hni_cpp/src/head_track_action_server2.cpp +++ b/hni_cpp/src/head_track_action_server2.cpp @@ -19,9 +19,9 @@ #include #include -#include // std::string, std::stof -#include // std::cout -#include // std::queue +#include // std::string, std::stof +#include // std::cout +#include // std::queue #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -37,111 +37,142 @@ #include "hni_cpp/head_track_action_server2.hpp" - - -namespace hni_head_track_action_server2 { - -HeadTrackActionServer2::HeadTrackActionServer2(const rclcpp::NodeOptions & options) - : rclcpp::Node("head_track_action_server2_node", options), - kSecToHeadReset_(6), kTrackMaxSize_(2), kHeadWidthStep_(0.36), kHeadHeightStep_(0.25), kVerResolution_(480), kHorResolution_(640) { - +namespace hni_head_track_action_server2 +{ + +HeadTrackActionServer2::HeadTrackActionServer2(const rclcpp::NodeOptions& options) + : rclcpp::Node("head_track_action_server2_node", options) + , kSecToHeadReset_(6) + , kTrackMaxSize_(2) + , kHeadWidthStep_(0.36) + , kHeadHeightStep_(0.25) + , kVerResolution_(480) + , kHorResolution_(640) +{ using namespace std::placeholders; - this->client_ptr_ = rclcpp_action::create_client( - this, - "face_tracker"); - - this->timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - std::bind(&HeadTrackActionServer2::sendGoal, this)); + this->client_ptr_ = rclcpp_action::create_client(this, "face_tracker"); + this->timer_ = + this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&HeadTrackActionServer2::sendGoal, this)); this->publisher_ = this->create_publisher("action_req2", 10); - this->action_server_ = rclcpp_action::create_server( - this, - "head_track", - std::bind(&HeadTrackActionServer2::handleGoal, this, _1, _2), - std::bind(&HeadTrackActionServer2::handleCancel, this, _1), - std::bind(&HeadTrackActionServer2::handleAccepted, this, _1)); - - std::vector c0 = {0.0, 0.0}; head_pos_[c0] = "c0"; - - std::vector u1 = { 0.0, -0.25 }; head_pos_[u1] = "u1"; - std::vector u2 = { 0.0, -0.50 }; head_pos_[u2] = "u2"; - - std::vector d1 = { 0.0, +0.25 }; head_pos_[d1] = "d1"; - std::vector d2 = { 0.0, +0.50 }; head_pos_[d2] = "d2"; - - std::vector l1 = { +0.36, 0.0 }; head_pos_[l1] = "l1"; - std::vector l2 = { +0.72, 0.0 }; head_pos_[l2] = "l2"; - std::vector l3 = { +1.08, 0.0 }; head_pos_[l3] = "l3"; - - std::vector r1 = { -0.36, 0.0 }; head_pos_[r1] = "r1"; - std::vector r2 = { -0.72, 0.0 }; head_pos_[r2] = "r2"; - std::vector r3 = { -1.08, 0.0 }; head_pos_[r3] = "r3"; - - std::vector u1r1 = { -0.36, -0.25 }; head_pos_[u1r1] = "u1r1"; - std::vector u1r2 = { -0.72, -0.25 }; head_pos_[u1r2] = "u1r2"; - std::vector u1r3 = { -1.08, -0.25 }; head_pos_[u1r3] = "u1r3"; - std::vector u2r1 = { -0.36, -0.50 }; head_pos_[u2r1] = "u2r1"; - std::vector u2r2 = { -0.72, -0.50 }; head_pos_[u2r1] = "u2r2"; - std::vector u2r3 = { -1.08, -0.50 }; head_pos_[u2r3] = "u2r3"; - - std::vector u1l1 = { +0.36, -0.25 }; head_pos_[u1l1] = "u1l1"; - std::vector u1l2 = { +0.72, -0.25 }; head_pos_[u1l2] = "u1l2"; - std::vector u1l3 = { +1.08, -0.25 }; head_pos_[u1l3] = "u1l3"; - std::vector u2l1 = { +0.36, -0.50 }; head_pos_[u2l1] = "u2l1"; - std::vector u2l2 = { +0.72, -0.50 }; head_pos_[u2l2] = "u2l2"; - std::vector u2l3 = { +1.08, -0.50 }; head_pos_[u2l3] = "u2l3"; - - std::vector d1l1 = { +0.36, +0.25 }; head_pos_[d1l1] = "d1l1"; - std::vector d1l2 = { +0.72, +0.25 }; head_pos_[d1l2] = "d1l2"; - std::vector d1l3 = { +1.08, +0.25 }; head_pos_[d1l3] = "d1l3"; - std::vector d2l1 = { +0.36, +0.50 }; head_pos_[d2l1] = "d2l1"; - std::vector d2l2 = { +0.72, +0.50 }; head_pos_[d2l2] = "d2l2"; - std::vector d2l3 = { +1.08, +0.50 }; head_pos_[d2l3] = "d2l3"; - - std::vector d1r1 = { -0.36, +0.25 }; head_pos_[d1r1] = "d1r1"; - std::vector d1r2 = { -0.72, +0.25 }; head_pos_[d1r2] = "d1r2"; - std::vector d1r3 = { -1.08, +0.25 }; head_pos_[d1r3] = "d1r3"; - std::vector d2r1 = { -0.36, +0.50 }; head_pos_[d2r1] = "d2r1"; - std::vector d2r2 = { -0.72, +0.50 }; head_pos_[d2r2] = "d2r2"; - std::vector d2r3 = { -1.08, +0.50 }; head_pos_[d2r3] = "d2r3"; - + this, "head_track", std::bind(&HeadTrackActionServer2::handleGoal, this, _1, _2), + std::bind(&HeadTrackActionServer2::handleCancel, this, _1), + std::bind(&HeadTrackActionServer2::handleAccepted, this, _1)); + + std::vector c0 = { 0.0, 0.0 }; + head_pos_[c0] = "c0"; + + std::vector u1 = { 0.0, -0.25 }; + head_pos_[u1] = "u1"; + std::vector u2 = { 0.0, -0.50 }; + head_pos_[u2] = "u2"; + + std::vector d1 = { 0.0, +0.25 }; + head_pos_[d1] = "d1"; + std::vector d2 = { 0.0, +0.50 }; + head_pos_[d2] = "d2"; + + std::vector l1 = { +0.36, 0.0 }; + head_pos_[l1] = "l1"; + std::vector l2 = { +0.72, 0.0 }; + head_pos_[l2] = "l2"; + std::vector l3 = { +1.08, 0.0 }; + head_pos_[l3] = "l3"; + + std::vector r1 = { -0.36, 0.0 }; + head_pos_[r1] = "r1"; + std::vector r2 = { -0.72, 0.0 }; + head_pos_[r2] = "r2"; + std::vector r3 = { -1.08, 0.0 }; + head_pos_[r3] = "r3"; + + std::vector u1r1 = { -0.36, -0.25 }; + head_pos_[u1r1] = "u1r1"; + std::vector u1r2 = { -0.72, -0.25 }; + head_pos_[u1r2] = "u1r2"; + std::vector u1r3 = { -1.08, -0.25 }; + head_pos_[u1r3] = "u1r3"; + std::vector u2r1 = { -0.36, -0.50 }; + head_pos_[u2r1] = "u2r1"; + std::vector u2r2 = { -0.72, -0.50 }; + head_pos_[u2r1] = "u2r2"; + std::vector u2r3 = { -1.08, -0.50 }; + head_pos_[u2r3] = "u2r3"; + + std::vector u1l1 = { +0.36, -0.25 }; + head_pos_[u1l1] = "u1l1"; + std::vector u1l2 = { +0.72, -0.25 }; + head_pos_[u1l2] = "u1l2"; + std::vector u1l3 = { +1.08, -0.25 }; + head_pos_[u1l3] = "u1l3"; + std::vector u2l1 = { +0.36, -0.50 }; + head_pos_[u2l1] = "u2l1"; + std::vector u2l2 = { +0.72, -0.50 }; + head_pos_[u2l2] = "u2l2"; + std::vector u2l3 = { +1.08, -0.50 }; + head_pos_[u2l3] = "u2l3"; + + std::vector d1l1 = { +0.36, +0.25 }; + head_pos_[d1l1] = "d1l1"; + std::vector d1l2 = { +0.72, +0.25 }; + head_pos_[d1l2] = "d1l2"; + std::vector d1l3 = { +1.08, +0.25 }; + head_pos_[d1l3] = "d1l3"; + std::vector d2l1 = { +0.36, +0.50 }; + head_pos_[d2l1] = "d2l1"; + std::vector d2l2 = { +0.72, +0.50 }; + head_pos_[d2l2] = "d2l2"; + std::vector d2l3 = { +1.08, +0.50 }; + head_pos_[d2l3] = "d2l3"; + + std::vector d1r1 = { -0.36, +0.25 }; + head_pos_[d1r1] = "d1r1"; + std::vector d1r2 = { -0.72, +0.25 }; + head_pos_[d1r2] = "d1r2"; + std::vector d1r3 = { -1.08, +0.25 }; + head_pos_[d1r3] = "d1r3"; + std::vector d2r1 = { -0.36, +0.50 }; + head_pos_[d2r1] = "d2r1"; + std::vector d2r2 = { -0.72, +0.50 }; + head_pos_[d2r2] = "d2r2"; + std::vector d2r3 = { -1.08, +0.50 }; + head_pos_[d2r3] = "d2r3"; RCLCPP_INFO(this->get_logger(), "HeadTrackActionServer2 Initialized"); - //this->play_cmd_sub_ = this->create_subscription( - // "face_pos_topic", 10, std::bind(&HeadTrackActionServer2::face_pos_callback, this, _1)); - - //this->obj_pos_sub_ = this->create_subscription( - // "face_tracker/_action/feedback", 10, std::bind(&HeadTrackActionServer2::obj_pos_callback, this, _1)); + // this->play_cmd_sub_ = this->create_subscription( + // "face_pos_topic", 10, std::bind(&HeadTrackActionServer2::face_pos_callback, this, _1)); + // this->obj_pos_sub_ = this->create_subscription( + // "face_tracker/_action/feedback", 10, std::bind(&HeadTrackActionServer2::obj_pos_callback, + // this, _1)); } -HeadTrackActionServer2::~HeadTrackActionServer2() {} +HeadTrackActionServer2::~HeadTrackActionServer2() +{ +} nao_lola_command_msgs::msg::JointIndexes joint_indexes_msg_; -uint8_t head_joint_indexes_ [2] = { joint_indexes_msg_.HEADYAW, - joint_indexes_msg_.HEADPITCH, - }; +uint8_t head_joint_indexes_[2] = { + joint_indexes_msg_.HEADYAW, + joint_indexes_msg_.HEADPITCH, +}; uint8_t num_rec_joints_ = sizeof(head_joint_indexes_) / sizeof(head_joint_indexes_[0]); - - - - - // ###################################### client ################################ -void HeadTrackActionServer2::sendGoal() { +void HeadTrackActionServer2::sendGoal() +{ using namespace std::placeholders; this->timer_->cancel(); - if (!this->client_ptr_->wait_for_action_server()) { + if (!this->client_ptr_->wait_for_action_server()) + { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } @@ -152,58 +183,58 @@ void HeadTrackActionServer2::sendGoal() { auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&HeadTrackActionServer2::goalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&HeadTrackActionServer2::goalResponseCallback, this, _1); - send_goal_options.feedback_callback = - std::bind(&HeadTrackActionServer2::feedbackCallback, this, _1, _2); + send_goal_options.feedback_callback = std::bind(&HeadTrackActionServer2::feedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&HeadTrackActionServer2::resultCallback, this, _1); + send_goal_options.result_callback = std::bind(&HeadTrackActionServer2::resultCallback, this, _1); - RCLCPP_INFO(this->get_logger(), "Sending obj tracking goal " ); + RCLCPP_INFO(this->get_logger(), "Sending obj tracking goal "); this->client_ptr_->async_send_goal(goal_msg, send_goal_options); } - -void HeadTrackActionServer2::goalResponseCallback( - const GoalHandleObjTrack::SharedPtr & goal_handle) { - if (!goal_handle) { +void HeadTrackActionServer2::goalResponseCallback(const GoalHandleObjTrack::SharedPtr& goal_handle) +{ + if (!goal_handle) + { RCLCPP_ERROR(this->get_logger(), "Goal was rejected by object tracking server"); - } else { + } + else + { RCLCPP_INFO(this->get_logger(), "Goal accepted by object tracking server, waiting for result"); } } -void HeadTrackActionServer2::feedbackCallback( - GoalHandleObjTrack::SharedPtr, - const std::shared_ptr feedback) { - - x_track_.push(feedback->center.x); //from 0 to kHorResolution_ +void HeadTrackActionServer2::feedbackCallback(GoalHandleObjTrack::SharedPtr, + const std::shared_ptr feedback) +{ + x_track_.push(feedback->center.x); // from 0 to kHorResolution_ - y_track_.push(feedback->center.y); //from 0 to kVerResolution_ + y_track_.push(feedback->center.y); // from 0 to kVerResolution_ - if (x_track_.size() > kTrackMaxSize_) { + if (x_track_.size() > kTrackMaxSize_) + { x_track_.pop(); y_track_.pop(); } - } -void HeadTrackActionServer2::resultCallback(const GoalHandleObjTrack::WrappedResult & result) { - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; +void HeadTrackActionServer2::resultCallback(const GoalHandleObjTrack::WrappedResult& result) +{ + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; } if (result.result->success) @@ -212,50 +243,48 @@ void HeadTrackActionServer2::resultCallback(const GoalHandleObjTrack::WrappedRes rclcpp::shutdown(); } - // ###################################### server ################################ - -rclcpp_action::GoalResponse HeadTrackActionServer2::handleGoal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) { - - RCLCPP_INFO( this->get_logger(), "Received goal request" ); +rclcpp_action::GoalResponse HeadTrackActionServer2::handleGoal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) +{ + RCLCPP_INFO(this->get_logger(), "Received goal request"); (void)uuid; - if (true) { - + if (true) + { return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - - } else { - - //RCLCPP_ERROR(this->get_logger(), ("Couldn't open file " + goal->path).c_str() ); - //fileSuccessfullyRead_ = false; + } + else + { + // RCLCPP_ERROR(this->get_logger(), ("Couldn't open file " + goal->path).c_str() ); + // fileSuccessfullyRead_ = false; return rclcpp_action::GoalResponse::REJECT; } - } -rclcpp_action::CancelResponse HeadTrackActionServer2::handleCancel( - const std::shared_ptr goal_handle) { +rclcpp_action::CancelResponse +HeadTrackActionServer2::handleCancel(const std::shared_ptr goal_handle) +{ RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; } -void HeadTrackActionServer2::handleAccepted(const std::shared_ptr goal_handle) { +void HeadTrackActionServer2::handleAccepted(const std::shared_ptr goal_handle) +{ using namespace std::placeholders; // this needs to return quickly to avoid blocking the executor, so spin up a new thread - std::thread{std::bind(&HeadTrackActionServer2::execute, this, _1), goal_handle} .detach(); + std::thread{ std::bind(&HeadTrackActionServer2::execute, this, _1), goal_handle }.detach(); } -void HeadTrackActionServer2::execute(const std::shared_ptr goal_handle) { - +void HeadTrackActionServer2::execute(const std::shared_ptr goal_handle) +{ RCLCPP_INFO(this->get_logger(), "Executing goal"); const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared(); - //auto percentage = feedback->center; + // auto percentage = feedback->center; auto result = std::make_shared(); float x; @@ -269,19 +298,18 @@ void HeadTrackActionServer2::execute(const std::shared_ptr bool first_miss_face = true, reset_head = false; rclcpp::Time noFaceTime; - std::vector cur_head = {0.0, 0.0}; + std::vector cur_head = { 0.0, 0.0 }; std::string pos_file = ""; + rclcpp::Rate loop_rate(0.5); // Hz - rclcpp::Rate loop_rate(0.5); //Hz - - - while (rclcpp::ok()) { - + while (rclcpp::ok()) + { RCLCPP_INFO(this->get_logger(), "inside while"); // Check if there is a cancel request - if (goal_handle->is_canceling()) { + if (goal_handle->is_canceling()) + { result->success = false; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Goal canceled"); @@ -292,127 +320,154 @@ void HeadTrackActionServer2::execute(const std::shared_ptr tracking = false; right_available = cur_yaw > -3 * kHeadWidthStep_; - left_available = cur_yaw < 3 * kHeadWidthStep_; - up_available = cur_pitch > -2 * kHeadHeightStep_; - down_available = cur_pitch < 2 * kHeadHeightStep_; + left_available = cur_yaw < 3 * kHeadWidthStep_; + up_available = cur_pitch > -2 * kHeadHeightStep_; + down_available = cur_pitch < 2 * kHeadHeightStep_; x = x_track_.back(); y = y_track_.back(); - RCLCPP_INFO(this->get_logger(), ("Received point (x,y) : (" + std::to_string(x) + ", " + std::to_string(y) + ")" ).c_str() ); + RCLCPP_INFO(this->get_logger(), + ("Received point (x,y) : (" + std::to_string(x) + ", " + std::to_string(y) + ")").c_str()); - if ( x != -1 && y != -1) { // face detected + if (x != -1 && y != -1) + { // face detected RCLCPP_INFO(this->get_logger(), "face detected"); - if (x < 0.25 * kHorResolution_ ) { - - if (y < 0.25 * kVerResolution_ ) { + if (x < 0.25 * kHorResolution_) + { + if (y < 0.25 * kVerResolution_) + { // upper left RCLCPP_INFO(this->get_logger(), "upper left"); both = true; - if (left_available) { + if (left_available) + { tracking = true; - //jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADYAW); - //jpos_cmd_.positions.push_back(last_yaw_ + 1 * kHeadWidthStep_); + // jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADYAW); + // jpos_cmd_.positions.push_back(last_yaw_ + 1 * kHeadWidthStep_); cur_yaw += kHeadWidthStep_; } - if (up_available) { + if (up_available) + { tracking = true; - //jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADPITCH); - //jpos_cmd_.positions.push_back(last_pitch_ - 1 * kHeadHeightStep_); + // jpos_cmd_.indexes.push_back(joint_indexes_msg_.HEADPITCH); + // jpos_cmd_.positions.push_back(last_pitch_ - 1 * kHeadHeightStep_); cur_pitch -= kHeadHeightStep_; } - - } else if (y > 0.75 * kVerResolution_) { + } + else if (y > 0.75 * kVerResolution_) + { // down left RCLCPP_INFO(this->get_logger(), "down left"); both = true; - if (left_available) { + if (left_available) + { tracking = true; cur_yaw += kHeadWidthStep_; } - if (down_available) { + if (down_available) + { tracking = true; cur_pitch += kHeadHeightStep_; } - - } else { + } + else + { // left RCLCPP_INFO(this->get_logger(), "left"); - if (left_available) { + if (left_available) + { tracking = true; cur_yaw += kHeadWidthStep_; } } - - } else if (x > 0.75 * kHorResolution_) { - if (y < 0.25 * kVerResolution_ ) { + } + else if (x > 0.75 * kHorResolution_) + { + if (y < 0.25 * kVerResolution_) + { // upper right RCLCPP_INFO(this->get_logger(), "upper right"); both = true; - if (right_available) { + if (right_available) + { tracking = true; cur_yaw -= kHeadWidthStep_; } - if (up_available) { + if (up_available) + { tracking = true; cur_pitch -= kHeadHeightStep_; } - - } else if (y > 0.75 * kVerResolution_) { - //down right + } + else if (y > 0.75 * kVerResolution_) + { + // down right RCLCPP_INFO(this->get_logger(), "down right"); both = true; - if (right_available) { + if (right_available) + { tracking = true; cur_yaw -= kHeadWidthStep_; } - if (down_available) { + if (down_available) + { tracking = true; cur_pitch += kHeadHeightStep_; } - - } else { + } + else + { // right RCLCPP_INFO(this->get_logger(), "right"); - if (right_available) { + if (right_available) + { tracking = true; cur_yaw -= kHeadWidthStep_; } } - } else if ( !both && y < 0.25 * kVerResolution_ ) { - //up + } + else if (!both && y < 0.25 * kVerResolution_) + { + // up RCLCPP_INFO(this->get_logger(), "up"); - if (up_available) { + if (up_available) + { tracking = true; cur_pitch -= kHeadHeightStep_; } - - } else if ( !both && y > 0.75 * kVerResolution_ ) { - //down + } + else if (!both && y > 0.75 * kVerResolution_) + { + // down RCLCPP_INFO(this->get_logger(), "down"); - if (down_available) { + if (down_available) + { tracking = true; cur_pitch += kHeadHeightStep_; } - } else { - //center of the frame + } + else + { + // center of the frame RCLCPP_INFO(this->get_logger(), "center"); - } // send command - if (tracking) { + if (tracking) + { cur_head.clear(); cur_head.emplace_back(cur_yaw); cur_head.emplace_back(cur_pitch); - if ( head_pos_.find(cur_head) != head_pos_.end() ) { + if (head_pos_.find(cur_head) != head_pos_.end()) + { pos_file = head_pos_[cur_head]; } auto message = std_msgs::msg::String(); @@ -421,7 +476,9 @@ void HeadTrackActionServer2::execute(const std::shared_ptr publisher_->publish(message); } // object position available - } else { + } + else + { RCLCPP_WARN(this->get_logger(), "No face detected"); /* @@ -439,14 +496,12 @@ void HeadTrackActionServer2::execute(const std::shared_ptr //jpos_pub_->publish(jpos_cmd_); reset_head = true; }*/ - } loop_rate.sleep(); - } } -} // namespace hni_head_track_action_server +} // namespace hni_head_track_action_server2 RCLCPP_COMPONENTS_REGISTER_NODE(hni_head_track_action_server2::HeadTrackActionServer2) \ No newline at end of file diff --git a/hni_cpp/src/joints_play_action_client.cpp b/hni_cpp/src/joints_play_action_client.cpp index 674a137..a62c2e8 100644 --- a/hni_cpp/src/joints_play_action_client.cpp +++ b/hni_cpp/src/joints_play_action_client.cpp @@ -17,16 +17,13 @@ namespace fs = boost::filesystem; +namespace hni_joints_play_action_client +{ -namespace hni_joints_play_action_client { - - -JointsPlayActionClient::JointsPlayActionClient(const rclcpp::NodeOptions & options) - : rclcpp::Node("joints_play_action_server_node", options) { - - this->client_ptr_ = rclcpp_action::create_client( - this, - "joints_play"); +JointsPlayActionClient::JointsPlayActionClient(const rclcpp::NodeOptions& options) + : rclcpp::Node("joints_play_action_server_node", options) +{ + this->client_ptr_ = rclcpp_action::create_client(this, "joints_play"); /*this->timer_ = this->create_wall_timer( std::chrono::milliseconds(500), @@ -35,10 +32,11 @@ JointsPlayActionClient::JointsPlayActionClient(const rclcpp::NodeOptions & optio this->declare_parameter("file", getDefaultFullFilePath()); RCLCPP_INFO(this->get_logger(), "JointsPlayActionClient initialized"); - } -JointsPlayActionClient::~JointsPlayActionClient() {} +JointsPlayActionClient::~JointsPlayActionClient() +{ +} /* void JointsPlayActionClient::sendGoal() { @@ -74,13 +72,14 @@ void JointsPlayActionClient::sendGoal() { this->client_ptr_->async_send_goal(goal_msg, send_goal_options); }*/ - -void JointsPlayActionClient::sendAsyncGoal(std::string & action_path) { +void JointsPlayActionClient::sendAsyncGoal(std::string& action_path) +{ using namespace std::placeholders; - //this->timer_->cancel(); + // this->timer_->cancel(); - if (!this->client_ptr_->wait_for_action_server()) { + if (!this->client_ptr_->wait_for_action_server()) + { RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); rclcpp::shutdown(); } @@ -91,23 +90,19 @@ void JointsPlayActionClient::sendAsyncGoal(std::string & action_path) { auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.goal_response_callback = - std::bind(&JointsPlayActionClient::goalResponseCallback, this, _1); + send_goal_options.goal_response_callback = std::bind(&JointsPlayActionClient::goalResponseCallback, this, _1); - send_goal_options.feedback_callback = - std::bind(&JointsPlayActionClient::feedbackCallback, this, _1, _2); + send_goal_options.feedback_callback = std::bind(&JointsPlayActionClient::feedbackCallback, this, _1, _2); - send_goal_options.result_callback = - std::bind(&JointsPlayActionClient::resultCallback, this, _1); + send_goal_options.result_callback = std::bind(&JointsPlayActionClient::resultCallback, this, _1); - RCLCPP_INFO(this->get_logger(), ("Sending goal: " + action_path).c_str() ); + RCLCPP_INFO(this->get_logger(), ("Sending goal: " + action_path).c_str()); this->client_ptr_->async_send_goal(goal_msg, send_goal_options); } - -std::string JointsPlayActionClient::getDefaultFullFilePath() { - +std::string JointsPlayActionClient::getDefaultFullFilePath() +{ /* RCLCPP_INFO(this->get_logger(),"getDefaultFullFilePath "); std::string file = "moves/hello.txt"; @@ -127,39 +122,42 @@ std::string JointsPlayActionClient::getDefaultFullFilePath() { */ return "/home/nao/rolling_ws/src/hni/hni_cpp/moves/fear.txt"; - //return "hello.txt"; //WORKING + // return "hello.txt"; //WORKING } - -void JointsPlayActionClient::goalResponseCallback(const GoalHandleJointsPlay::SharedPtr & goal_handle) { - if (!goal_handle) { +void JointsPlayActionClient::goalResponseCallback(const GoalHandleJointsPlay::SharedPtr& goal_handle) +{ + if (!goal_handle) + { RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); - } else { + } + else + { RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); } } -void JointsPlayActionClient::feedbackCallback( - GoalHandleJointsPlay::SharedPtr, - const std::shared_ptr feedback) { - - //TODO - +void JointsPlayActionClient::feedbackCallback(GoalHandleJointsPlay::SharedPtr, + const std::shared_ptr feedback) +{ + // TODO } -void JointsPlayActionClient::resultCallback(const GoalHandleJointsPlay::WrappedResult & result) { - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); - return; - default: - RCLCPP_ERROR(this->get_logger(), "Unknown result code"); - return; +void JointsPlayActionClient::resultCallback(const GoalHandleJointsPlay::WrappedResult& result) +{ + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; } if (result.result->success) diff --git a/hni_cpp/src/joints_play_action_server.cpp b/hni_cpp/src/joints_play_action_server.cpp index e3f2275..00ab45a 100644 --- a/hni_cpp/src/joints_play_action_server.cpp +++ b/hni_cpp/src/joints_play_action_server.cpp @@ -5,9 +5,9 @@ #include #include #include -#include // std::string, std::stof std::c_str +#include // std::string, std::stof std::c_str -#include // std::cout +#include // std::cout #include "hni_interfaces/action/joints_play.hpp" #include "rclcpp/rclcpp.hpp" @@ -19,102 +19,104 @@ #include "hni_cpp/joints_play_action_server.hpp" -namespace hni_joints_play_action_server { +namespace hni_joints_play_action_server +{ -JointsPlayActionServer::JointsPlayActionServer(const rclcpp::NodeOptions & options) - : rclcpp::Node("joints_play_action_server_node", options) { +JointsPlayActionServer::JointsPlayActionServer(const rclcpp::NodeOptions& options) + : rclcpp::Node("joints_play_action_server_node", options) +{ using namespace std::placeholders; this->action_server_ = rclcpp_action::create_server( - this, - "joints_play", - std::bind(&JointsPlayActionServer::handleGoal, this, _1, _2), - std::bind(&JointsPlayActionServer::handleCancel, this, _1), - std::bind(&JointsPlayActionServer::handleAccepted, this, _1)); + this, "joints_play", std::bind(&JointsPlayActionServer::handleGoal, this, _1, _2), + std::bind(&JointsPlayActionServer::handleCancel, this, _1), + std::bind(&JointsPlayActionServer::handleAccepted, this, _1)); - this->jpos_pub_ = this->create_publisher( - "effectors/joint_positions", 10); + this->jpos_pub_ = this->create_publisher("effectors/joint_positions", 10); - this->jstiff_pub_ = this->create_publisher( - "effectors/joint_stiffnesses", 10); + this->jstiff_pub_ = + this->create_publisher("effectors/joint_stiffnesses", 10); free_ = true; RCLCPP_INFO(this->get_logger(), "JointsPlayActionServer Initialized"); } -JointsPlayActionServer::~JointsPlayActionServer() {} +JointsPlayActionServer::~JointsPlayActionServer() +{ +} nao_lola_command_msgs::msg::JointIndexes joint_indexes_msg_; -uint8_t rec_joint_indexes_ [10] = { joint_indexes_msg_.LSHOULDERPITCH, - joint_indexes_msg_.LSHOULDERROLL, - joint_indexes_msg_.LELBOWYAW, - joint_indexes_msg_.LELBOWROLL, - joint_indexes_msg_.LWRISTYAW, - joint_indexes_msg_.RSHOULDERPITCH, - joint_indexes_msg_.RSHOULDERROLL, - joint_indexes_msg_.RELBOWYAW, - joint_indexes_msg_.RELBOWROLL, - joint_indexes_msg_.RWRISTYAW - }; +uint8_t rec_joint_indexes_[10] = { joint_indexes_msg_.LSHOULDERPITCH, joint_indexes_msg_.LSHOULDERROLL, + joint_indexes_msg_.LELBOWYAW, joint_indexes_msg_.LELBOWROLL, + joint_indexes_msg_.LWRISTYAW, joint_indexes_msg_.RSHOULDERPITCH, + joint_indexes_msg_.RSHOULDERROLL, joint_indexes_msg_.RELBOWYAW, + joint_indexes_msg_.RELBOWROLL, joint_indexes_msg_.RWRISTYAW }; uint8_t num_rec_joints_ = sizeof(rec_joint_indexes_) / sizeof(rec_joint_indexes_[0]); -rclcpp_action::GoalResponse JointsPlayActionServer::handleGoal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) { - - RCLCPP_INFO( this->get_logger(), ("Received goal request with file path: " + goal->path).c_str() ); +rclcpp_action::GoalResponse JointsPlayActionServer::handleGoal(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) +{ + RCLCPP_INFO(this->get_logger(), ("Received goal request with file path: " + goal->path).c_str()); (void)uuid; - //check the bool variable to avoid concurrency + // check the bool variable to avoid concurrency - if (free_.load()) { + if (free_.load()) + { free_.store(false); - ifs_.open( goal->path, std::ifstream::in); - if (ifs_.is_open()) { - //RCLCPP_WARN( this->get_logger(), ("File succesfully loaded from " + goal->path).c_str() ); + ifs_.open(goal->path, std::ifstream::in); + if (ifs_.is_open()) + { + // RCLCPP_WARN( this->get_logger(), ("File succesfully loaded from " + goal->path).c_str() ); fileSuccessfullyRead_ = true; recorded_joints_.clear(); float joint_value; std::string line; - while (!ifs_.eof()) { + while (!ifs_.eof()) + { std::getline(ifs_, line, '\n'); joint_value = std::stof(line); - recorded_joints_.emplace_back( joint_value ); + recorded_joints_.emplace_back(joint_value); } ifs_.close(); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } else { - RCLCPP_ERROR(this->get_logger(), ("Couldn't open file " + goal->path).c_str() ); + } + else + { + RCLCPP_ERROR(this->get_logger(), ("Couldn't open file " + goal->path).c_str()); fileSuccessfullyRead_ = false; free_.store(true); return rclcpp_action::GoalResponse::REJECT; } - } else { - RCLCPP_ERROR(this->get_logger(), "Action server already executing " ); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Action server already executing "); return rclcpp_action::GoalResponse::REJECT; } - } -rclcpp_action::CancelResponse JointsPlayActionServer::handleCancel( - const std::shared_ptr goal_handle) { +rclcpp_action::CancelResponse +JointsPlayActionServer::handleCancel(const std::shared_ptr goal_handle) +{ RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); (void)goal_handle; return rclcpp_action::CancelResponse::ACCEPT; } -void JointsPlayActionServer::handleAccepted(const std::shared_ptr goal_handle) { +void JointsPlayActionServer::handleAccepted(const std::shared_ptr goal_handle) +{ using namespace std::placeholders; // this needs to return quickly to avoid blocking the executor, so spin up a new thread - std::thread{std::bind(&JointsPlayActionServer::execute, this, _1), goal_handle} .detach(); + std::thread{ std::bind(&JointsPlayActionServer::execute, this, _1), goal_handle }.detach(); } -void JointsPlayActionServer::execute(const std::shared_ptr goal_handle) { - +void JointsPlayActionServer::execute(const std::shared_ptr goal_handle) +{ RCLCPP_INFO(this->get_logger(), "Executing goal"); const auto goal = goal_handle->get_goal(); @@ -122,7 +124,8 @@ void JointsPlayActionServer::execute(const std::shared_ptr auto percentage = feedback->percentage; auto result = std::make_shared(); - for (auto i : rec_joint_indexes_) { + for (auto i : rec_joint_indexes_) + { jstiff_cmd_.indexes.push_back(i); jstiff_cmd_.stiffnesses.push_back(1.0); } @@ -133,17 +136,19 @@ void JointsPlayActionServer::execute(const std::shared_ptr // We create a Rate object of 82Hz rclcpp::Rate loop_rate(82); - for (uint64_t i = 0; i < recorded_joints_.size() && rclcpp::ok(); i += num_rec_joints_) { - + for (uint64_t i = 0; i < recorded_joints_.size() && rclcpp::ok(); i += num_rec_joints_) + { // Check if there is a cancel request - if (goal_handle->is_canceling()) { + if (goal_handle->is_canceling()) + { result->success = false; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Goal canceled"); return; } - for (uint8_t j = 0; j < num_rec_joints_; j++ ) { + for (uint8_t j = 0; j < num_rec_joints_; j++) + { jpos_cmd_.indexes.push_back(rec_joint_indexes_[j]); jpos_cmd_.positions.push_back(recorded_joints_[i + j]); } @@ -153,11 +158,12 @@ void JointsPlayActionServer::execute(const std::shared_ptr jpos_cmd_.positions.clear(); loop_rate.sleep(); - //rclcpp::sleep_for(12ms); // 80hz nao_lola update rate + // rclcpp::sleep_for(12ms); // 80hz nao_lola update rate } // Check if goal is done - if (rclcpp::ok()) { + if (rclcpp::ok()) + { result->success = true; goal_handle->succeed(result); free_.store(true); diff --git a/hni_cpp/src/record.cpp b/hni_cpp/src/record.cpp index 2bd8e87..7eab7e4 100644 --- a/hni_cpp/src/record.cpp +++ b/hni_cpp/src/record.cpp @@ -5,8 +5,7 @@ #include #include #include -#include // std::string, std::stof - +#include // std::string, std::stof #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/bool.hpp" @@ -19,33 +18,30 @@ using std::placeholders::_1; using namespace std::chrono_literals; -class JointsRecorder : public rclcpp::Node { - - public: - - JointsRecorder() - : Node("puppeteer") { +class JointsRecorder : public rclcpp::Node +{ +public: + JointsRecorder() : Node("puppeteer") + { jpos_sub_ = this->create_subscription( - "sensors/joint_positions", 10, std::bind(&JointsRecorder::jpos_callback, this, _1)); + "sensors/joint_positions", 10, std::bind(&JointsRecorder::jpos_callback, this, _1)); record_cmd_sub_ = this->create_subscription( - "record_cmd_topic", 10, std::bind(&JointsRecorder::record_cmd_callback, this, _1)); + "record_cmd_topic", 10, std::bind(&JointsRecorder::record_cmd_callback, this, _1)); play_cmd_sub_ = this->create_subscription( - "play_cmd_topic", 10, std::bind(&JointsRecorder::play_cmd_callback, this, _1)); + "play_cmd_topic", 10, std::bind(&JointsRecorder::play_cmd_callback, this, _1)); - jpos_pub_ = this->create_publisher( - "effectors/joint_positions", 10); - jstiff_pub_ = this->create_publisher( - "effectors/joint_stiffnesses", 10); + jpos_pub_ = this->create_publisher("effectors/joint_positions", 10); + jstiff_pub_ = + this->create_publisher("effectors/joint_stiffnesses", 10); - //jstiff_cmd_.indexes.push_back(nao_lola_sensor_msgs::msg::JointIndexes::LHIPYAWPITCH); - //jstiff_cmd_.stiffnesses.push_back(1.0); - //jstiff_pub_->publish(jstiff_cmd_); + // jstiff_cmd_.indexes.push_back(nao_lola_sensor_msgs::msg::JointIndexes::LHIPYAWPITCH); + // jstiff_cmd_.stiffnesses.push_back(1.0); + // jstiff_pub_->publish(jstiff_cmd_); RCLCPP_INFO_STREAM(this->get_logger(), "JointsRecorder node initialized"); } - private: - +private: /*enum movingJoints : unsigned int { LSHOULDERPITCH = nao_lola_sensor_msgs::msg::JointIndexes::LSHOULDERPITCH; LSHOULDERROLL = nao_lola_sensor_msgs::msg::JointIndexes::LSHOULDERROLL; @@ -68,17 +64,11 @@ class JointsRecorder : public rclcpp::Node { bool recording_ = false; nao_lola_sensor_msgs::msg::JointIndexes joint_indexes_msg_; - uint8_t rec_joint_indexes_ [10] = { joint_indexes_msg_.LSHOULDERPITCH, - joint_indexes_msg_.LSHOULDERROLL, - joint_indexes_msg_.LELBOWYAW, - joint_indexes_msg_.LELBOWROLL, - joint_indexes_msg_.LWRISTYAW, - joint_indexes_msg_.RSHOULDERPITCH, - joint_indexes_msg_.RSHOULDERROLL, - joint_indexes_msg_.RELBOWYAW, - joint_indexes_msg_.RELBOWROLL, - joint_indexes_msg_.RWRISTYAW - }; + uint8_t rec_joint_indexes_[10] = { joint_indexes_msg_.LSHOULDERPITCH, joint_indexes_msg_.LSHOULDERROLL, + joint_indexes_msg_.LELBOWYAW, joint_indexes_msg_.LELBOWROLL, + joint_indexes_msg_.LWRISTYAW, joint_indexes_msg_.RSHOULDERPITCH, + joint_indexes_msg_.RSHOULDERROLL, joint_indexes_msg_.RELBOWYAW, + joint_indexes_msg_.RELBOWROLL, joint_indexes_msg_.RWRISTYAW }; uint8_t num_rec_joints_ = sizeof(rec_joint_indexes_) / sizeof(rec_joint_indexes_[0]); std::vector record_; @@ -86,28 +76,27 @@ class JointsRecorder : public rclcpp::Node { nao_lola_command_msgs::msg::JointPositions jpos_cmd_; nao_lola_command_msgs::msg::JointStiffnesses jstiff_cmd_; - - - void jpos_callback(const nao_lola_sensor_msgs::msg::JointPositions & joints) { - - if (recording_) { - - for (auto i : rec_joint_indexes_) { + void jpos_callback(const nao_lola_sensor_msgs::msg::JointPositions& joints) + { + if (recording_) + { + for (auto i : rec_joint_indexes_) + { record_.emplace_back(joints.positions[i]); } RCLCPP_DEBUG_STREAM(this->get_logger(), "New joint positions saved"); - } - } - void record_cmd_callback(const std_msgs::msg::Bool & msg) { + void record_cmd_callback(const std_msgs::msg::Bool& msg) + { RCLCPP_INFO_STREAM(this->get_logger(), "I heard: '" << msg.data << "' from record_cmd_sub"); bool cmd = msg.data; - if (!recording_ && cmd) { - - for (int i : rec_joint_indexes_) { + if (!recording_ && cmd) + { + for (int i : rec_joint_indexes_) + { jstiff_cmd_.indexes.push_back(i); jstiff_cmd_.stiffnesses.push_back(0.0); } @@ -120,15 +109,18 @@ class JointsRecorder : public rclcpp::Node { recording_ = true; RCLCPP_INFO_STREAM(this->get_logger(), "Start recording"); - - } else if (recording_ && !cmd) { + } + else if (recording_ && !cmd) + { // save the data recording_ = false; RCLCPP_INFO_STREAM(this->get_logger(), "Stop recording"); file_.open("vector_file_.txt", std::ios_base::out); - if (file_.is_open()) { - for (uint i = 0; i < record_.size(); i++) { //-1 to avoid a final \n line + if (file_.is_open()) + { + for (uint i = 0; i < record_.size(); i++) + { //-1 to avoid a final \n line if (i < record_.size() - 1) file_ << record_[i] << '\n'; else @@ -137,57 +129,61 @@ class JointsRecorder : public rclcpp::Node { file_.close(); RCLCPP_INFO_STREAM(this->get_logger(), "Joint position saved to file"); - - - } else + } + else std::cout << "Unable to open file"; } } - void play_cmd_callback(const std_msgs::msg::Bool & msg) { - + void play_cmd_callback(const std_msgs::msg::Bool& msg) + { RCLCPP_INFO_STREAM(this->get_logger(), "I heard: '" << msg.data << "' from play_cmd_sub"); - //rclcpp::sleep_for(1s); + // rclcpp::sleep_for(1s); bool cmd = msg.data; - //RCLCPP_WARN_STREAM(this->get_logger(), "I heard: '" << cmd); - //rclcpp::sleep_for(1s); - + // RCLCPP_WARN_STREAM(this->get_logger(), "I heard: '" << cmd); + // rclcpp::sleep_for(1s); - if (!recording_ && cmd) { - - for (auto i : rec_joint_indexes_) { + if (!recording_ && cmd) + { + for (auto i : rec_joint_indexes_) + { jstiff_cmd_.indexes.push_back(i); jstiff_cmd_.stiffnesses.push_back(1.0); } RCLCPP_DEBUG_STREAM(this->get_logger(), "Publishing 1.0 on effectors/joint_stiffnesses"); - //rclcpp::sleep_for(1s); + // rclcpp::sleep_for(1s); jstiff_pub_->publish(jstiff_cmd_); jstiff_cmd_.indexes.clear(); jstiff_cmd_.stiffnesses.clear(); - - //file_.open("vector_file_.txt", std::ios_base::in); + // file_.open("vector_file_.txt", std::ios_base::in); file_.open("vector_file_.txt", std::fstream::in); - if (file_.is_open()) { + if (file_.is_open()) + { record_.clear(); float joint_value; std::string line; - while (!file_.eof()) { + while (!file_.eof()) + { std::getline(file_, line, '\n'); - //std::cout << line; + // std::cout << line; joint_value = std::stof(line); - record_.emplace_back( joint_value ); + record_.emplace_back(joint_value); } file_.close(); - } else { + } + else + { return; } RCLCPP_INFO_STREAM(this->get_logger(), "Publishing on effectors/joint_positions 82 Hz"); rclcpp::Rate loop_rate(82); - for (uint64_t i = 0; i < record_.size(); i += num_rec_joints_) { - for (uint8_t j = 0; j < num_rec_joints_; j++ ) { + for (uint64_t i = 0; i < record_.size(); i += num_rec_joints_) + { + for (uint8_t j = 0; j < num_rec_joints_; j++) + { jpos_cmd_.indexes.push_back(rec_joint_indexes_[j]); jpos_cmd_.positions.push_back(record_[i + j]); } @@ -197,18 +193,16 @@ class JointsRecorder : public rclcpp::Node { loop_rate.sleep(); - //rclcpp::sleep_for(12ms); // 80hz nao_lola update rate + // rclcpp::sleep_for(12ms); // 80hz nao_lola update rate } RCLCPP_INFO_STREAM(this->get_logger(), "Play finished."); - } } - }; - -int main(int argc, char * argv[]) { +int main(int argc, char* argv[]) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/hni_interfaces/CMakeLists.txt b/hni_interfaces/CMakeLists.txt index dd57f62..6054755 100644 --- a/hni_interfaces/CMakeLists.txt +++ b/hni_interfaces/CMakeLists.txt @@ -26,11 +26,11 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) + #set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) # comment the line when this package is in a git repo and when # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) + #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/hni_interfaces/package.xml b/hni_interfaces/package.xml index f1f741b..c2a86c0 100644 --- a/hni_interfaces/package.xml +++ b/hni_interfaces/package.xml @@ -8,16 +8,15 @@ Apache License 2.0 ament_cmake + + ament_lint_auto + ament_lint_common geometry_msgs rosidl_default_generators rosidl_default_runtime rosidl_interface_packages - - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/hni_py/package.xml b/hni_py/package.xml index cde584b..226d852 100644 --- a/hni_py/package.xml +++ b/hni_py/package.xml @@ -6,7 +6,10 @@ TODO: Package description antbono Apache-2.0 - + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest hni_interfaces rclpy std_msgs @@ -14,10 +17,7 @@ nao_lola_command_msgs - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest + ament_python