40 #include <urdf_parser/urdf_parser.h> 42 #include <gtest/gtest.h> 43 #include <boost/filesystem/path.hpp> 46 #include <moveit_resources/config.h> 53 boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
57 std::fstream xml_file((res_path /
"pr2_description/urdf/robot.xml").
string().c_str(), std::fstream::in);
58 if (xml_file.is_open())
60 while (xml_file.good())
63 std::getline(xml_file, line);
64 xml_string += (line +
"\n");
97 static const std::string SMODEL1 =
"<?xml version=\"1.0\" ?>" 98 "<robot name=\"pr2\">" 99 "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" " 100 "parent_frame=\"base_footprint\" type=\"planar\"/>" 108 static const std::string SMODEL2 =
"<?xml version=\"1.0\" ?>" 109 "<robot name=\"pr2\">" 110 "<virtual_joint name=\"world_joint\" child_link=\"base_footprint\" " 111 "parent_frame=\"odom_combined\" type=\"floating\"/>" 122 static const std::string SMODEL1 =
"<?xml version=\"1.0\" ?>" 123 "<robot name=\"pr2\">" 124 "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" " 125 "parent_frame=\"base_footprint\" type=\"planar\"/>" 126 "<group name=\"left_arm_base_tip\">" 127 "<chain base_link=\"monkey_base\" tip_link=\"monkey_tip\"/>" 129 "<group name=\"left_arm_joints\">" 130 "<joint name=\"l_monkey_pan_joint\"/>" 131 "<joint name=\"l_monkey_fles_joint\"/>" 140 ASSERT_TRUE(left_arm_base_tip_group ==
nullptr);
143 ASSERT_TRUE(left_arm_joints_group ==
nullptr);
145 static const std::string SMODEL2 =
"<?xml version=\"1.0\" ?>" 146 "<robot name=\"pr2\">" 147 "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" " 148 "parent_frame=\"base_footprint\" type=\"planar\"/>" 149 "<group name=\"left_arm_base_tip\">" 150 "<chain base_link=\"torso_lift_link\" tip_link=\"l_wrist_roll_link\"/>" 152 "<group name=\"left_arm_joints\">" 153 "<joint name=\"l_shoulder_pan_joint\"/>" 154 "<joint name=\"l_shoulder_lift_joint\"/>" 155 "<joint name=\"l_upper_arm_roll_joint\"/>" 156 "<joint name=\"l_elbow_flex_joint\"/>" 157 "<joint name=\"l_forearm_roll_joint\"/>" 158 "<joint name=\"l_wrist_flex_joint\"/>" 159 "<joint name=\"l_wrist_roll_joint\"/>" 166 left_arm_base_tip_group = robot_model2->getJointModelGroup(
"left_arm_base_tip");
167 ASSERT_TRUE(left_arm_base_tip_group !=
nullptr);
169 left_arm_joints_group = robot_model2->getJointModelGroup(
"left_arm_joints");
170 ASSERT_TRUE(left_arm_joints_group !=
nullptr);
178 EXPECT_EQ(robot_model2->getVariableNames().size(), robot_model2->getVariableCount());
180 bool found_shoulder_pan_link =
false;
181 bool found_wrist_roll_link =
false;
182 for (
unsigned int i = 0; i < left_arm_base_tip_group->
getLinkModels().size(); i++)
184 if (left_arm_base_tip_group->
getLinkModels()[i]->getName() ==
"l_shoulder_pan_link")
187 found_shoulder_pan_link =
true;
189 if (left_arm_base_tip_group->
getLinkModels()[i]->getName() ==
"l_wrist_roll_link")
192 found_wrist_roll_link =
true;
197 EXPECT_TRUE(found_wrist_roll_link);
201 std::map<std::string, double> jv;
202 jv[
"base_joint/x"] = 0.433;
203 jv[
"base_joint/theta"] = -0.5;
226 EXPECT_EQ(jmg2->getSubgroupNames().size(), 5);
234 EXPECT_TRUE(model->getLinkModel(
"r_gripper_palm_link")->getAssociatedFixedTransforms().size() > 1);
247 std::vector<shapes::ShapeConstPtr>
shapes;
251 poses.push_back(Eigen::Affine3d::Identity());
252 std::set<std::string> touch_links;
254 trajectory_msgs::JointTrajectory empty_state;
256 robot_model->getLinkModel(
"r_gripper_palm_link"),
"box", shapes, poses, touch_links, empty_state);
259 std::vector<const moveit::core::AttachedBody*> attached_bodies_1;
261 ASSERT_EQ(attached_bodies_1.size(), 1);
263 std::vector<const moveit::core::AttachedBody*> attached_bodies_2;
266 ASSERT_EQ(attached_bodies_2.size(), 1);
269 attached_bodies_1.clear();
271 ASSERT_EQ(attached_bodies_1.size(), 0);
274 attached_bodies_2.clear();
276 ASSERT_EQ(attached_bodies_2.size(), 0);
279 int main(
int argc,
char** argv)
281 testing::InitGoogleTest(&argc, argv);
282 return RUN_ALL_TESTS();
bool isSubgroup(const std::string &group) const
Check if the joints of group group are a subset of the joints in this group.
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
const std::string & getModelFrame() const
Get the frame in which the transforms for this model are computed (when using a RobotState). This frame depends on the root joint. As such, the frame is either extracted from SRDF, or it is assumed to be the name of the root link.
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Core components of MoveIt!
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up the joints included in this group. The number of returned...
const std::vector< std::string > & getSubgroupNames() const
Get the names of the groups that are subsets of this one (in terms of joints set) ...
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void attachBody(AttachedBody *attached_body)
Add an attached body to this state. Ownership of the memory for the attached body is assumed by the s...
const JointModel * getRootJoint() const
Get the root joint. There will be one root joint unless the model is empty. This is either extracted ...
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully. If you change these values externally you need to make sure you trigger a forced update for the state by calling update(true).
#define EXPECT_NEAR(a, b, prec)
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state...
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
urdf::ModelInterfaceSharedPtr urdf_model
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt! robot state...
def xml_string(rootXml, addHeader=True)
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt! robot state to a robot state message.
srdf::ModelSharedPtr srdf_model
TEST_F(LoadPlanningModelsPr2, InitOK)
int main(int argc, char **argv)
Representation of a robot's state. This includes position, velocity, acceleration and effort...
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
moveit::core::RobotModelConstPtr robot_model
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
#define EXPECT_TRUE(args)
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)...
std::shared_ptr< const Shape > ShapeConstPtr