69 srdf::Model::GroupState* searched_state =
nullptr;
71 for (srdf::Model::GroupState& state :
srdf_config_->getGroupStates())
73 if (state.name_ == name && state.group_ == group)
75 searched_state = &state;
80 return searched_state;
92 for (
const auto& disabled_collision :
srdf_config_->getDisabledCollisions())
104 moveit_msgs::msg::DisplayRobotState msg;
121 moveit::core::RobotModelPtr robot_model =
srdf_config_->getRobotModel();
122 if (!robot_model->hasJointModelGroup(group_name))
124 throw std::runtime_error(std::string(
"Unable to find joint model group for group: ") + group_name +
125 ". Are you sure this group has associated joints/links?");
130 std::vector<const moveit::core::JointModel*> joint_models;
133 if (joint_model->getVariableCount() != 1 ||
134 joint_model->isPassive() ||
135 joint_model->getMimic())
138 joint_models.push_back(joint_model);
146 group_state.joint_values_.clear();
148 const auto& robot_state =
srdf_config_->getPlanningScene()->getCurrentState();
152 std::vector<double> joint_values(joint_model->getVariableCount());
153 const double*
const first_variable = robot_state.getVariablePositions() + joint_model->getFirstVariableIndex();
154 std::copy(first_variable, first_variable + joint_values.size(), joint_values.begin());
157 group_state.joint_values_[joint_model->getName()] = std::move(joint_values);
void clear()
Clear the allowed collision matrix.
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
Set an entry corresponding to a pair of elements.
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Representation of a robot's state. This includes position, velocity, acceleration and effort.
rclcpp::Node::SharedPtr parent_node_
void setToCurrentValues(srdf::Model::GroupState &group_state)
void loadAllowedCollisionMatrix()
Load the allowed collision matrix from the SRDF's list of link pairs.
std::vector< const moveit::core::JointModel * > getSimpleJointModels(const std::string &group_name) const
Returns a vector of joint models for the given group name.
void onInit() override
Overridable initialization method.
void publishState(const moveit::core::RobotState &robot_state)
Publish the given state on the moveit_robot_state topic.
collision_detection::AllowedCollisionMatrix allowed_collision_matrix_
Allowed collision matrix for robot poses.
srdf::Model::GroupState * findPoseByName(const std::string &name, const std::string &group)
collision_detection::CollisionRequest request_
rclcpp::Publisher< moveit_msgs::msg::DisplayRobotState >::SharedPtr pub_robot_state_
Remember the publisher for quick publishing later.
bool checkSelfCollision(const moveit::core::RobotState &robot_state)
Check if the given robot state is in collision.
void onInit() override
Overridable initialization method.
std::shared_ptr< SRDFConfig > srdf_config_
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
bool verbose
Flag indicating whether information about detected collisions should be reported.
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
std::size_t max_contacts
Overall maximum number of contacts to compute.
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
Representation of a collision checking result.