diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index a5282a05ea..0a7cb7340c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -34,6 +34,8 @@ #include +#include + #include #include #include @@ -75,7 +77,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin // goal given in joint space if (!req.goal_constraints.front().joint_constraints.empty()) { - info.link_name = robot_model_->getJointModelGroup(req.group_name)->getSolverInstance()->getTipFrame(); + info.link_name = getSolverTipFrame(robot_model_->getJointModelGroup(req.group_name)); if (req.goal_constraints.front().joint_constraints.size() != robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size()) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp index 3ad29859c1..b514e0afde 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp @@ -220,6 +220,10 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver) const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_); const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance(); + if (!solver) + { + throw("No IK solver configured for group '" + planning_group_ + "'"); + } // robot state moveit::core::RobotState rstate(robot_model_); diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp index f57cae4d1c..6c8319c5fc 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp @@ -418,7 +418,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan ControllersMap::iterator c = managed_controllers_.find(it); if (c != managed_controllers_.end()) { // controller belongs to this manager - request->stop_controllers.push_back(c->second.name); + request->deactivate_controllers.push_back(c->second.name); claimed_resources.right.erase(c->second.name); // remove resources } } @@ -430,16 +430,16 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan ControllersMap::iterator c = managed_controllers_.find(it); if (c != managed_controllers_.end()) { // controller belongs to this manager - request->start_controllers.push_back(c->second.name); + request->activate_controllers.push_back(c->second.name); for (const auto& required_resource : c->second.required_command_interfaces) { resources_bimap::right_const_iterator res = claimed_resources.right.find(required_resource); if (res != claimed_resources.right.end()) { // resource is claimed - if (std::find(request->stop_controllers.begin(), request->stop_controllers.end(), res->second) == - request->stop_controllers.end()) + if (std::find(request->deactivate_controllers.begin(), request->deactivate_controllers.end(), + res->second) == request->deactivate_controllers.end()) { - request->stop_controllers.push_back(res->second); // add claiming controller to stop list + request->deactivate_controllers.push_back(res->second); // add claiming controller to stop list } claimed_resources.left.erase(res->second); // remove claimed resources } @@ -451,7 +451,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan // successfully activated or deactivated. request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; - if (!request->start_controllers.empty() || !request->stop_controllers.empty()) + if (!request->activate_controllers.empty() || !request->deactivate_controllers.empty()) { // something to switch? auto result_future = switch_controller_service_->async_send_request(request); if (result_future.wait_for(std::chrono::duration(SERVICE_CALL_TIMEOUT)) == std::future_status::timeout) diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h index 311d51f7a5..e6e21efe97 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h @@ -145,7 +145,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase do { status = result_future.wait_for(50ms); - if ((status == std::future_status::timeout) and ((node_->now() - start) > timeout)) + if ((status == std::future_status::timeout) && ((node_->now() - start) > timeout)) { RCLCPP_WARN(LOGGER, "waitForExecution timed out"); return false; diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp index 30b93ce190..e9bb41b4e2 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp @@ -109,6 +109,7 @@ void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningScen color.r = c.r; color.g = c.g; color.b = c.b; + color.a = c.a; alpha = c.a; } for (std::size_t j = 0; j < object->shapes_.size(); ++j)