Group '' was not found, yet part of srdf
Hello, I'm trying to use the move_base_to_manip package on my mobile manipulator.
I am using ROS Melodic and installed the package via git clone. I changed the move group name to the relevant group name ur_arm
(which is declared in the srdf) in the move_base_to_manip.cpp
.
When I, as mentioned on the website, do rosrun move_base_to_manip provide_target
and roslaunch move_base_to_manip move_base_to_manip.launch
the client and service find each other but the following error occurs in the terminal of the roslaunch move_base_to_manip move_base_to_manip.launch
:
[FATAL] [1588166074.398821886, 707.054000000]: Group '' was not found. terminate called after throwing an instance of 'std::runtime_error' what(): Group '' was not found.
move_base_to_manip.cpp:
# include "move_base_to_manip.h"
/////////////////
// The algorithm:
/////////////////
// Put the end-effector at the height & orientation of the desired pose.
// Calculate the X,Y vector from the ee's current pose to the desired pose.
// Plan a Cartesian move to the grasp pose. What % completes? If 100%, we're done!
// Based on the completed Cartesian %, how far along the (X,Y) vector must the base move?
// Move the base to that position.
// Should be able to reach it now. Run the program again to complete the motion or make minor
// corrections as needed.
///////////////////////////////////////////////////
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_base_to_manip");
// Start an action server
base_planner commander;
// Wait for an action goal to trigger the CB
ros::spin();
return 0;
}
//////////////
// Constructor
//////////////
base_planner::base_planner():
as_(nh_, "move_base_to_manip", boost::bind(&base_planner::do_motion_CB, this, _1), false)
{
// Set node parameters, if they aren't defined in a launch file
set_node_params();
// Action server
as_.start();
}
/////////////////////////////////////////////////////
// Once a goal is received, start planning and moving
/////////////////////////////////////////////////////
void base_planner::do_motion_CB( const move_base_to_manip::desired_poseGoalConstPtr& goal )
{
ROS_INFO("[move_base_to_manip] Received a new goal. Moving.");
// Initialize MoveGroup for the arm
std::string move_group_name;
nh_.getParam("ur_arm", move_group_name);
moveit::planning_interface::MoveGroupInterface move_group( move_group_name );
// get the current EE pose
geometry_msgs::PoseStamped start_pose = move_group.getCurrentPose();
// make sure the goal is in the right frame
std::string base_frame_name;
nh_.getParam("base_link", base_frame_name);
move_base_to_manip::desired_poseResult result;
if (goal->desired_pose.header.frame_id != base_frame_name)
{
result.success = false;
as_.setSucceeded( result );
ROS_WARN_STREAM("The target pose should be given in " << base_frame_name);
return;
}
// We don't want to move in (X,Y), initially
geometry_msgs::PoseStamped desired_height_orient = goal->desired_pose;
desired_height_orient.pose.position.x = start_pose.pose.position.x;
desired_height_orient.pose.position.y = start_pose.pose.position.y;
///////////////////////////////////////////////////
// Put the EE at the height & orientation we desire
///////////////////////////////////////////////////
ROS_INFO_STREAM("[move_base_to_manip] Moving to the desired height and orientation.");
move_group.setPoseTarget( desired_height_orient );
// Get the orientation as RPY so we can manipulate it
tf::Quaternion gripper_quat;
tf::quaternionMsgToTF( desired_height_orient.pose.orientation, gripper_quat );
double object_roll, object_pitch, object_yaw;
tf::Matrix3x3(gripper_quat).getRPY(object_roll, object_pitch, object_yaw);
moveit::planning_interface::MoveGroupInterface::Plan move_plan;
PLAN_AGAIN:
bool ok_to_flip;
nh_.getParam("ok_to_flip", ok_to_flip);
if ( !move_group.plan(move_plan) && ok_to_flip ) // If it fails, try spinning the gripper 180deg
{
geometry_msgs::Quaternion gripper_quat_msg = tf::createQuaternionMsgFromRollPitchYaw( 0., 0., object_yaw +3.14159);
desired_height_orient.pose.orientation = gripper_quat_msg;
move_group.setPoseTarget( desired_height_orient );
if ( !move_group.plan(move_plan) ) // If it fails again, try spinning the gripper -180deg from the original attempt
{
gripper_quat_msg = tf::createQuaternionMsgFromRollPitchYaw( 0., 0., object_yaw -3.14159 ...