91 auto scene{ planning_scene_monitor_->getPlanningScene() };
92 moveit_msgs::msg::PlanningScene msg;
93 msg.is_diff = msg.robot_state.is_diff =
true;
94 planning_scene_monitor_->newPlanningSceneMessage(msg);
95 EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene());
96 msg.is_diff = msg.robot_state.is_diff =
false;
97 planning_scene_monitor_->newPlanningSceneMessage(msg);
98 EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene());
114 moveit_msgs::msg::PlanningScene msg;
115 msg.is_diff = msg.robot_state.is_diff =
true;
118 msg.fixed_frame_transforms.emplace_back();
119 msg.fixed_frame_transforms.back().header.frame_id =
"base_link";
120 msg.fixed_frame_transforms.back().child_frame_id =
"object";
121 msg.fixed_frame_transforms.back().transform.rotation.w = 1.0;
123 msg.fixed_frame_transforms.clear();
125 msg.robot_state.is_diff =
true;
128 msg.robot_state.is_diff =
false;
129 TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE | UpdateType::UPDATE_GEOMETRY);
131 msg.robot_state = moveit_msgs::msg::RobotState{};
132 msg.robot_state.is_diff =
true;
133 moveit_msgs::msg::CollisionObject collision_object;
134 collision_object.header.frame_id =
"base_link";
135 collision_object.id =
"object";
136 collision_object.operation = moveit_msgs::msg::CollisionObject::ADD;
137 collision_object.pose.orientation.w = 1.0;
138 collision_object.primitives.emplace_back();
139 collision_object.primitives.back().type = shape_msgs::msg::SolidPrimitive::SPHERE;
140 collision_object.primitives.back().dimensions = { 1.0 };
141 msg.world.collision_objects.emplace_back(collision_object);
144 msg.world.collision_objects.clear();