SMACC2
Public Member Functions | Private Member Functions | Private Attributes | List of all members
cl_move_group_interface::CbMoveKnownState Class Reference

#include <cb_move_known_state.hpp>

Inheritance diagram for cl_move_group_interface::CbMoveKnownState:
Inheritance graph
Collaboration diagram for cl_move_group_interface::CbMoveKnownState:
Collaboration graph

Public Member Functions

 CbMoveKnownState (std::string pkg, std::string config_path)
 
virtual ~CbMoveKnownState ()
 
void onEntry () override
 
- Public Member Functions inherited from cl_move_group_interface::CbMoveJoints
 CbMoveJoints ()
 
 CbMoveJoints (const std::map< std::string, double > &jointValueTarget)
 
virtual void onEntry () override
 
virtual void onExit () override
 
- Public Member Functions inherited from smacc2::SmaccAsyncClientBehavior
template<typename TOrthogonal , typename TSourceObject >
void onOrthogonalAllocation ()
 
virtual ~SmaccAsyncClientBehavior ()
 
template<typename TCallback , typename T >
boost::signals2::connection onSuccess (TCallback callback, T *object)
 
template<typename TCallback , typename T >
boost::signals2::connection onFinished (TCallback callback, T *object)
 
template<typename TCallback , typename T >
boost::signals2::connection onFailure (TCallback callback, T *object)
 
- Public Member Functions inherited from smacc2::ISmaccClientBehavior
 ISmaccClientBehavior ()
 
virtual ~ISmaccClientBehavior ()
 
ISmaccStateMachinegetStateMachine ()
 
std::string getName () const
 
template<typename SmaccClientType >
void requiresClient (SmaccClientType *&storage)
 
template<typename SmaccComponentType >
void requiresComponent (SmaccComponentType *&storage)
 

Private Member Functions

std::map< std::string, double > loadJointStatesFromFile (std::string pkg, std::string filepath)
 

Private Attributes

std::string pkg_
 
std::string config_path_
 

Additional Inherited Members

- Public Attributes inherited from cl_move_group_interface::CbMoveJoints
std::optional< double > scalingFactor_
 
std::map< std::string, double > jointValueTarget_
 
std::optional< std::string > group_
 
- Protected Member Functions inherited from cl_move_group_interface::CbMoveJoints
void moveJoints (moveit::planning_interface::MoveGroupInterface &moveGroupInterface)
 
- Protected Member Functions inherited from smacc2::SmaccAsyncClientBehavior
void postSuccessEvent ()
 
void postFailureEvent ()
 
virtual void dispose () override
 
bool isShutdownRequested ()
 
- Protected Member Functions inherited from smacc2::ISmaccClientBehavior
virtual void runtimeConfigure ()
 
virtual void onEntry ()
 
virtual void onExit ()
 
template<typename EventType >
void postEvent (const EventType &ev)
 
template<typename EventType >
void postEvent ()
 
ISmaccStategetCurrentState ()
 
virtual void dispose ()
 
virtual rclcpp::Node::SharedPtr getNode ()
 
virtual rclcpp::Logger getLogger ()
 
- Protected Attributes inherited from cl_move_group_interface::CbMoveJoints
ClMoveGroupmovegroupClient_
 

Detailed Description

Definition at line 29 of file cb_move_known_state.hpp.

Constructor & Destructor Documentation

◆ CbMoveKnownState()

cl_move_group_interface::CbMoveKnownState::CbMoveKnownState ( std::string  pkg,
std::string  config_path 
)

Definition at line 29 of file cb_move_known_state.cpp.

◆ ~CbMoveKnownState()

cl_move_group_interface::CbMoveKnownState::~CbMoveKnownState ( )
virtual

Definition at line 34 of file cb_move_known_state.cpp.

34{}

Member Function Documentation

◆ loadJointStatesFromFile()

std::map< std::string, double > cl_move_group_interface::CbMoveKnownState::loadJointStatesFromFile ( std::string  pkg,
std::string  filepath 
)
private

Definition at line 43 of file cb_move_known_state.cpp.

45{
46 //auto pkgpath = ros::package::getPath(pkg);
47 std::string pkgpath = ament_index_cpp::get_package_share_directory(pkg);
48
49 std::map<std::string, double> jointStates;
50
51 if (pkgpath == "")
52 {
53 RCLCPP_ERROR_STREAM(
54 getLogger(), "[" << getName() << "] package not found for the known poses file: " << pkg
55 << std::endl
56 << " [IGNORING BEHAVIOR]");
57 return jointStates;
58 }
59
60 filepath = pkgpath + "/" + filepath;
61
62 RCLCPP_INFO_STREAM(
63 getLogger(), "[" << getName() << "] Opening file with joint known state: " << filepath);
64
65 if (std::filesystem::exists(filepath))
66 {
67 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] known state file exists: " << filepath);
68 }
69 else
70 {
71 RCLCPP_ERROR_STREAM(
72 getLogger(), "[" << getName() << "] known state file does not exists: " << filepath);
73 }
74
75 std::ifstream ifs(filepath.c_str(), std::ifstream::in);
76 if (ifs.good() == false)
77 {
78 RCLCPP_ERROR_STREAM(
79 getLogger(),
80 "[" << getName() << "] Error opening file with joint known states: " << filepath);
81 throw std::string("joint state files not found");
82 }
83
84 try
85 {
86#ifdef HAVE_NEW_YAMLCPP
87 YAML::Node node = YAML::Load(ifs);
88#else
89 YAML::Parser parser(ifs);
90 parser.GetNextDocument(node);
91#endif
92
93#ifdef HAVE_NEW_YAMLCPP
94 const YAML::Node & wp_node_tmp = node["joint_states"];
95 const YAML::Node * wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
96#else
97 const YAML::Node * wp_node = node.FindValue("waypoints");
98#endif
99
100 if (wp_node != NULL)
101 {
102 try
103 {
104 for (YAML::const_iterator it = wp_node->begin(); it != wp_node->end(); ++it)
105 {
106 std::string key = it->first.as<std::string>();
107 double value = it->second.as<double>();
108 RCLCPP_DEBUG_STREAM(getLogger(), " joint - " << key << ": " << value);
109 jointStates[key] = value;
110 }
111
112 return jointStates;
113 }
114 catch (std::exception & ex)
115 {
116 RCLCPP_ERROR(getLogger(), "trying to convert to map, failed, errormsg: %s", ex.what());
117 }
118
119 RCLCPP_INFO_STREAM(getLogger(), "Parsed " << jointStates.size() << " joint entries.");
120 }
121 else
122 {
123 RCLCPP_WARN_STREAM(getLogger(), "Couldn't find any jointStates in the provided yaml file.");
124 }
125 }
126 catch (const YAML::ParserException & ex)
127 {
128 RCLCPP_ERROR_STREAM(
129 getLogger(), "Error loading the Waypoints YAML file. Incorrect syntax: " << ex.what());
130 }
131 return jointStates;
132}

References smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), and generate_debs::parser.

Referenced by onEntry().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ onEntry()

void cl_move_group_interface::CbMoveKnownState::onEntry ( )
overridevirtual

Reimplemented from cl_move_group_interface::CbMoveJoints.

Definition at line 36 of file cb_move_known_state.cpp.

37{
40}
std::map< std::string, double > jointValueTarget_
std::map< std::string, double > loadJointStatesFromFile(std::string pkg, std::string filepath)

References config_path_, cl_move_group_interface::CbMoveJoints::jointValueTarget_, loadJointStatesFromFile(), cl_move_group_interface::CbMoveJoints::onEntry(), and pkg_.

Here is the call graph for this function:

Member Data Documentation

◆ config_path_

std::string cl_move_group_interface::CbMoveKnownState::config_path_
private

Definition at line 41 of file cb_move_known_state.hpp.

Referenced by onEntry().

◆ pkg_

std::string cl_move_group_interface::CbMoveKnownState::pkg_
private

Definition at line 40 of file cb_move_known_state.hpp.

Referenced by onEntry().


The documentation for this class was generated from the following files: