cl_moveit2z::CbMoveKnownState Class Reference

#include <cb_move_known_state.hpp>

Inheritance diagram for cl_moveit2z::CbMoveKnownState:

Collaboration diagram for cl_moveit2z::CbMoveKnownState:

Public Member Functions
	CbMoveKnownState (std::string pkg, std::string config_path)
virtual	~CbMoveKnownState ()
void	onEntry () override
Public Member Functions inherited from cl_moveit2z::CbMoveJoints
	CbMoveJoints ()
	CbMoveJoints (const std::map< std::string, double > &jointValueTarget)
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)
void	requestForceFinish ()
void	executeOnEntry () override
void	executeOnExit () override
void	waitOnEntryThread (bool requestFinish)
template<typename TCallbackMethod , typename T >
boost::signals2::connection	onSuccess (TCallbackMethod callback, T *object)
template<typename TCallbackMethod , typename T >
boost::signals2::connection	onFinished (TCallbackMethod callback, T *object)
template<typename TCallbackMethod , typename T >
boost::signals2::connection	onFailure (TCallbackMethod callback, T *object)
Public Member Functions inherited from smacc2::ISmaccClientBehavior
	ISmaccClientBehavior ()
virtual	~ISmaccClientBehavior ()
ISmaccStateMachine *	getStateMachine ()
std::string	getName () const
template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)
template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage, bool throwExceptionIfNotExist=false)

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_moveit2z::CbMoveJoints
std::optional< double >	scalingFactor_
std::map< std::string, double >	jointValueTarget_
std::optional< std::string >	group_
Protected Member Functions inherited from cl_moveit2z::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 ()
	onEntry is executed in a new thread. However the current state cannot be left until the onEntry thread finishes. This flag can be checked from the onEntry thread to force finishing the thread.
Protected Member Functions inherited from smacc2::ISmaccClientBehavior
virtual void	runtimeConfigure ()
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
ISmaccState *	getCurrentState ()
virtual rclcpp::Node::SharedPtr	getNode () const
virtual rclcpp::Logger	getLogger () const
Protected Attributes inherited from cl_moveit2z::CbMoveJoints
ClMoveit2z *	movegroupClient_

Detailed Description

Definition at line 29 of file cb_move_known_state.hpp.

Constructor & Destructor Documentation

CbMoveKnownState()

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

Definition at line 29 of file cb_move_known_state.cpp.

: pkg_(pkg), config_path_(config_path)
{
}

~CbMoveKnownState()

cl_moveit2z::CbMoveKnownState::~CbMoveKnownState ( )

virtual

Definition at line 34 of file cb_move_known_state.cpp.

{}

Member Function Documentation

loadJointStatesFromFile()

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

private

Definition at line 43 of file cb_move_known_state.cpp.

{
  //auto pkgpath = ros::package::getPath(pkg);
  std::string pkgpath = ament_index_cpp::get_package_share_directory(pkg);
 
  std::map<std::string, double> jointStates;
 
  if (pkgpath == "")
  {
    RCLCPP_ERROR_STREAM(
      getLogger(), "[" << getName() << "] package not found for the known poses file: " << pkg
                       << std::endl
                       << " [IGNORING BEHAVIOR]");
    return jointStates;
  }
 
  filepath = pkgpath + "/" + filepath;
 
  RCLCPP_INFO_STREAM(
    getLogger(), "[" << getName() << "] Opening file with joint known state: " << filepath);
 
  if (std::filesystem::exists(filepath))
  {
    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] known state file exists: " << filepath);
  }
  else
  {
    RCLCPP_ERROR_STREAM(
      getLogger(), "[" << getName() << "] known state file does not exists: " << filepath);
  }
 
  std::ifstream ifs(filepath.c_str(), std::ifstream::in);
  if (ifs.good() == false)
  {
    RCLCPP_ERROR_STREAM(
      getLogger(),
      "[" << getName() << "] Error opening file with joint known states: " << filepath);
    throw std::string("joint state files not found");
  }
 
  try
  {
#ifdef HAVE_NEW_YAMLCPP
    YAML::Node node = YAML::Load(ifs);
#else
    YAML::Parser parser(ifs);
    parser.GetNextDocument(node);
#endif
 
#ifdef HAVE_NEW_YAMLCPP
    const YAML::Node & wp_node_tmp = node["joint_states"];
    const YAML::Node * wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
#else
    const YAML::Node * wp_node = node.FindValue("waypoints");
#endif
 
    if (wp_node != NULL)
    {
      try
      {
        for (YAML::const_iterator it = wp_node->begin(); it != wp_node->end(); ++it)
        {
          std::string key = it->first.as<std::string>();
          double value = it->second.as<double>();
          RCLCPP_DEBUG_STREAM(getLogger(), " joint - " << key << ": " << value);
          jointStates[key] = value;
        }
 
        return jointStates;
      }
      catch (std::exception & ex)
      {
        RCLCPP_ERROR(getLogger(), "trying to convert to map, failed, errormsg: %s", ex.what());
      }
 
      RCLCPP_INFO_STREAM(getLogger(), "Parsed " << jointStates.size() << " joint entries.");
    }
    else
    {
      RCLCPP_WARN_STREAM(getLogger(), "Couldn't find any jointStates in the provided yaml file.");
    }
  }
  catch (const YAML::ParserException & ex)
  {
    RCLCPP_ERROR_STREAM(
      getLogger(), "Error loading the Waypoints YAML file. Incorrect syntax: " << ex.what());
  }
  return jointStates;
}

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

Referenced by onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

onEntry()

void cl_moveit2z::CbMoveKnownState::onEntry ( )

overridevirtual

Reimplemented from cl_moveit2z::CbMoveJoints.

Definition at line 36 of file cb_move_known_state.cpp.

{
  jointValueTarget_ = loadJointStatesFromFile(pkg_, config_path_);
  CbMoveJoints::onEntry();
}

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

Here is the call graph for this function:

Member Data Documentation

config_path_

std::string cl_moveit2z::CbMoveKnownState::config_path_

private

Definition at line 41 of file cb_move_known_state.hpp.

Referenced by onEntry().

pkg_

std::string cl_moveit2z::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:

• smacc2_client_library/moveit2z_client/include/moveit2z_client/client_behaviors/cb_move_known_state.hpp
• smacc2_client_library/moveit2z_client/src/moveit2z_client/client_behaviors/cb_move_known_state.cpp