#include <cb_move_joints.hpp>

Inheritance diagram for cl_move_group_interface::CbMoveJoints:

Collaboration diagram for cl_move_group_interface::CbMoveJoints:

Public Member Functions
	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 ()

ISmaccStateMachine *	getStateMachine ()

std::string	getName () const

template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)

template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage)

Public Attributes
std::optional< double >	scalingFactor_

std::map< std::string, double >	jointValueTarget_

std::optional< std::string >	group_

Protected Member Functions
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 ()

ISmaccState *	getCurrentState ()

virtual void	dispose ()

virtual rclcpp::Node::SharedPtr	getNode ()

virtual rclcpp::Logger	getLogger ()

Protected Attributes
ClMoveGroup *	movegroupClient_

Detailed Description

Definition at line 31 of file cb_move_joints.hpp.

Constructor & Destructor Documentation

◆ CbMoveJoints() [1/2]

cl_move_group_interface::CbMoveJoints::CbMoveJoints ( )

Definition at line 31 of file cb_move_joints.cpp.

31{}

◆ CbMoveJoints() [2/2]

cl_move_group_interface::CbMoveJoints::CbMoveJoints ( const std::map< std::string, double > & jointValueTarget )

Definition at line 26 of file cb_move_joints.cpp.

: jointValueTarget_(jointValueTarget)
{
}

Member Function Documentation

◆ moveJoints()

void cl_move_group_interface::CbMoveJoints::moveJoints ( moveit::planning_interface::MoveGroupInterface & moveGroupInterface )

protected

Definition at line 72 of file cb_move_joints.cpp.

{
  if (scalingFactor_) moveGroupInterface.setMaxVelocityScalingFactor(*scalingFactor_);
 
  bool success;
  moveit::planning_interface::MoveGroupInterface::Plan computedMotionPlan;
 
  if (jointValueTarget_.size() == 0)
  {
    RCLCPP_WARN(
      getLogger(), "[CbMoveJoints] No joint was value specified. Skipping planning call.");
    success = false;
  }
  else
  {
    moveGroupInterface.setJointValueTarget(jointValueTarget_);
    //moveGroupInterface.setGoalJointTolerance(0.01);
    success =
      (moveGroupInterface.plan(computedMotionPlan) ==
       moveit::planning_interface::MoveItErrorCode::SUCCESS);
    RCLCPP_INFO(getLogger(), "Success Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
  }
 
  if (success)
  {
    auto executionResult = moveGroupInterface.execute(computedMotionPlan);
 
    auto statestr = currentJointStatesToString(moveGroupInterface, jointValueTarget_);
 
    if (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
    {
      RCLCPP_INFO_STREAM(
        getLogger(), "[" << this->getName()
                         << "] motion execution succeeded. Throwing success event. " << std::endl
                         << statestr);
      movegroupClient_->postEventMotionExecutionSucceded();
      this->postSuccessEvent();
    }
    else
    {
      RCLCPP_WARN_STREAM(
        getLogger(), "[" << this->getName() << "] motion execution failed. Throwing fail event."
                         << std::endl
                         << statestr);
      movegroupClient_->postEventMotionExecutionFailed();
      this->postFailureEvent();
    }
  }
  else
  {
    auto statestr = currentJointStatesToString(moveGroupInterface, jointValueTarget_);
    RCLCPP_WARN_STREAM(
      getLogger(), "[" << this->getName() << "] motion execution failed. Throwing fail event."
                       << std::endl
                       << statestr);
    movegroupClient_->postEventMotionExecutionFailed();
    this->postFailureEvent();
  }
}

References cl_move_group_interface::currentJointStatesToString(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), jointValueTarget_, movegroupClient_, cl_move_group_interface::ClMoveGroup::postEventMotionExecutionFailed(), cl_move_group_interface::ClMoveGroup::postEventMotionExecutionSucceded(), smacc2::SmaccAsyncClientBehavior::postFailureEvent(), smacc2::SmaccAsyncClientBehavior::postSuccessEvent(), and scalingFactor_.

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::CbMoveJoints::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Reimplemented in cl_move_group_interface::CbMoveKnownState, and cl_move_group_interface::CbMoveLastTrajectoryInitialState.

Definition at line 33 of file cb_move_joints.cpp.

{
  this->requiresClient(movegroupClient_);
 
  if (this->group_)
  {
    moveit::planning_interface::MoveGroupInterface move_group(
      getNode(), moveit::planning_interface::MoveGroupInterface::Options(*(this->group_)));
    this->moveJoints(move_group);
  }
  else
  {
    this->moveJoints(*movegroupClient_->moveGroupClientInterface);
  }
}

References smacc2::ISmaccClientBehavior::getNode(), group_, movegroupClient_, cl_move_group_interface::ClMoveGroup::moveGroupClientInterface, moveJoints(), and smacc2::ISmaccClientBehavior::requiresClient().

Referenced by cl_move_group_interface::CbMoveKnownState::onEntry(), and cl_move_group_interface::CbMoveLastTrajectoryInitialState::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onExit()

void cl_move_group_interface::CbMoveJoints::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 132 of file cb_move_joints.cpp.

132{}

Member Data Documentation

◆ group_

std::optional<std::string> cl_move_group_interface::CbMoveJoints::group_

Definition at line 36 of file cb_move_joints.hpp.

Referenced by onEntry().

◆ jointValueTarget_

std::map<std::string, double> cl_move_group_interface::CbMoveJoints::jointValueTarget_

Definition at line 35 of file cb_move_joints.hpp.

Referenced by moveJoints(), cl_move_group_interface::CbMoveKnownState::onEntry(), and cl_move_group_interface::CbMoveLastTrajectoryInitialState::onEntry().

◆ movegroupClient_

ClMoveGroup* cl_move_group_interface::CbMoveJoints::movegroupClient_

protected

Definition at line 45 of file cb_move_joints.hpp.

Referenced by moveJoints(), and onEntry().

◆ scalingFactor_

std::optional<double> cl_move_group_interface::CbMoveJoints::scalingFactor_

Definition at line 34 of file cb_move_joints.hpp.

Referenced by moveJoints().

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

smacc2_client_library/move_group_interface_client/include/move_group_interface_client/client_behaviors/cb_move_joints.hpp
smacc2_client_library/move_group_interface_client/src/move_group_interface_client/client_behaviors/cb_move_joints.cpp

Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes