I've created a new robot model for integration with moveit and am having trouble getting moveit to recognize the end effectors. I've generated the SRDF as well as other configuration files with the moveit setup assistant, and then launch the demo automatically generated along with my own node called servo capture. In my node I wait for rviz to come up, instantiate a move group similar to the PR2 move group interface tutorial, and then have a listener callback that sets a goal when a point is published from rviz. When the callback executes an error is displayed right after I print confirmation that the point was received:
[ INFO] [1397094975.183158794]: Got new goal: [1.72786,2.13589,0.553876]
[ERROR] [1397094975.183266964]: No end-effector to set the pose for
[ INFO] [1397094975.184236157]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ WARN] [1397094975.186156762]: No goal constraints specified. There is no problem to solve.
I've tried printing out the parent link name and and end effector name but both member functions to do so return empty strings.
Of the different ways I've configuring my planning groups in the setup assistant, the most reasonable one I could come up with is in the background below. With my current understanding the end effector group should have no links in common with its parent, be a child group of the arm, and defined as an end effector attached to the last link of the arm. It also needs to have its own kinematics solver written if it is not a chain, I've avoided this problem by just fixing the non chain components. If I launch the demo launch file which starts a joint publisher, transform publisher, state publisher, move group server and rviz, they should provide all resources necessary to run a fully functional move group interface node. If something was wrong with the end effector description I would still expect that the end of the chain link for the arm would at least be the default end effector. Is there anything in the above that seems like a misunderstanding?
Background:
Here is a visualization of the URDF:
And here is how I have the arm configured in the SRDF (through the setup assistant):
<group name="arm">
<link name="base_link" />
<link name="base" />
<link name="shoulder" />
<link name="arm" />
<link name="boom0" />
<link name="boom1" />
<link name="boom2" />
<link name="boom3" />
<link name="boom4" />
<link name="boom5" />
<link name="boom6" />
<link name="boom7" />
<link name="boom8" />
<link name="boom9" />
<link name="boom10" />
<link name="boom_cap" />
<joint name="base_link_to_base" />
<joint name="base_to_shoulder" />
<joint name="shoulder_to_arm" />
<joint name="arm_to_boom" />
<joint name="boom0_to_boom1" />
<joint name="boom1_to_boom2" />
<joint name="boom2_to_boom3" />
<joint name="boom3_to_boom4" />
<joint name="boom4_to_boom5" />
<joint name="boom5_to_boom6" />
<joint name="boom6_to_boom7" />
<joint name="boom7_to_boom8" />
<joint name="boom8_to_boom9" />
<joint name="boom9_to_boom10" />
<joint name="boom_to_boom_cap" />
<chain base_link="base" tip_link="boom_cap" />
<group name="electrostatic_gripper" />
</group>
<group name="electrostatic_gripper">
<link name="top_pad" />
<link name="left_pad" />
<link name="right_pad" />
<link name="bottom_pad" />
<link name="rocker_pin" />
<link name="rocker_frame" />
<joint name="rocker_frame_to_bottom_pad" />
<joint name="rocker_frame_to_left_pad" />
<joint name="rocker_frame_to_right_pad" />
<joint name="rocker_frame_to_top_pad" />
<joint name="boom_cap_to_rocker_pin" />
<joint name="rocker_pin_to_rocker_frame" />
<chain base_link="boom_cap" tip_link="rocker_frame" />
</group>
<end_effector name="electrostatic_gripper" parent_link="boom_cap" group="electrostatic_gripper" parent_group="arm" />
I'm fairly certain that the demo correctly loads the kinematics for my node once I call the demo.launch, but to be safe I also load them in the context of that node:
<launch>
<include file="$(find altius_arm)/launch/demo.launch"/>
<arg name="debug" default="true" />
<node name="servo_capture" pkg="altius_arm" type="servo_capture" required="false" output="screen">
<rosparam command="load" file="$(find altius_arm)/config/kinematics.yaml"/>
<rosparam command="load" file="$(find altius_arm)/config/ompl_planning.yaml"/>
<rosparam command="load" file="$(find altius_arm)/config/joint_limits.yaml"/>
<rosparam command="load" file="$(find altius_arm)/config/fake_controllers.yaml"/>
</node>
</launch>
When my node starts it prints out the end effector name and end effector link on launch but neither one has any value despite seeming to load correctly:
[ INFO] [1397094934.400980727]: Ready to take MoveGroup commands for group arm.
[ INFO] [1397094937.115176897]: Loading robot model 'altius_arm_satellite'...
[ INFO] [1397094937.115443436]: Loading robot model 'altius_arm_satellite'...
[ INFO] [1397094938.725159150]: Loading robot model 'altius_arm_satellite'...
[ INFO] [1397094938.755955440]: Loading robot model 'altius_arm_satellite'...
[ INFO] [1397094940.005700657]: Loading robot model 'altius_arm_satellite'...
[ INFO] [1397094940.030302217]: Loading robot model 'altius_arm_satellite'...
[ INFO] [1397094941.355940819]: Starting scene monitor
[ INFO] [1397094941.357702162]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1397094943.227471676]: Starting scene monitor
[ INFO] [1397094943.227555435]: Constructing new MoveGroup connection for group 'arm'
[ INFO] [1397094943.229115965]: Listening to '/planning_scene'
[ INFO] [1397094944.120048076]: Ready to take MoveGroup commands for group arm.
[ INFO] [1397094944.120161345]: Looking around: no
[ INFO] [1397094944.120224969]: Replanning: no
[ INFO] [1397094959.407079126]: Starting Servo Capture Node
[ INFO] [1397094959.407156366]: End effector:
[ INFO] [1397094959.407190909]: End effector link:
Here is a condensed version of my code:
class ServoCapture
{
private:
ros::NodeHandle node;
ros::Subscriber point_goal;
moveit::planning_interface::MoveGroup arm;
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit::planning_interface::MoveGroup::Plan capture_plan;
ros::Publisher display_publisher;
moveit_msgs::DisplayTrajectory display_trajectory;
geometry_msgs::Pose goal_pose;
public:
ServoCapture() : arm("arm")
{
sleep(25);
point_goal = node.subscribe("clicked_point",10,&ServoCapture::setNewGoal,this);
display_publisher = node.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
ROS_INFO_STREAM("Starting Servo Capture Node");
ROS_INFO_STREAM("End effector: " << arm.getEndEffector());
ROS_INFO_STREAM("End effector link: " << arm.getEndEffectorLink());
while (ros::ok())
{
sleep(1);
}
}
void setNewGoal(const geometry_msgs::PointStamped::ConstPtr& msg)
{
ROS_INFO_STREAM("Got new goal: [" << msg->point.x << "," << msg->point.y << "," << msg->point.z << "]");
goal_pose.position = msg->point;
arm.setPositionTarget(msg->point.x,msg->point.y,msg->point.z);
if (!arm.plan(capture_plan))
ROS_INFO_STREAM("Failed to plan point to goal");
else
{
display_trajectory.trajectory_start = capture_plan.start_state_;
display_trajectory.trajectory.push_back(capture_plan.trajectory_);
display_publisher.publish(display_trajectory);
}
}
};
int main(int argc, char *argv[])
{
// Initialize the node
ros::init(argc,argv,"servo_capture");
// Create a single asynchronous thread
ros::AsyncSpinner spinner(1);
// Start the callback managment thread
spinner.start();
// Initialize the servo capture
ServoCapture servo_capture;
// The capture routine has finished
ros::shutdown();
// Return no error executing capture attempt
return 0;
}
Originally posted by St3am on ROS Answers with karma: 170 on 2014-04-09
Post score: 0