How comunicate to arm_navigation node of ros electric
I have a problem and do not know how to solve. I am using ros, with its new arm_navigation, launched on roslaunch, and then not as moving the robot. I try to run this code to communicate with the move_arm, but I can not move anything
ros::init (argc, argv, "move_arm_pose_goal_test");
ros::NodeHandle nh;
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_arm",true);
move_arm.waitForServer();
ROS_INFO("Connected to server");
arm_navigation_msgs::MoveArmGoal goalA;
goalA.planner_service_name="/environment_server/set_planning_scene_diff";
goalA.planning_scene_diff.robot_state.joint_state.name.resize(7);
goalA.planning_scene_diff.robot_state.joint_state.name[0]="wam1";
goalA.planning_scene_diff.robot_state.joint_state.name[1]="wam2";
goalA.planning_scene_diff.robot_state.joint_state.name[2]="wam3";
goalA.planning_scene_diff.robot_state.joint_state.name[3]="wam4";
goalA.planning_scene_diff.robot_state.joint_state.name[4]="wam5";
goalA.planning_scene_diff.robot_state.joint_state.name[5]="wam6";
goalA.planning_scene_diff.robot_state.joint_state.name[6]="wam7";
goalA.planning_scene_diff.robot_state.joint_state.position.resize(7);
goalA.planning_scene_diff.robot_state.joint_state.position[0]=0.0;
goalA.planning_scene_diff.robot_state.joint_state.position[1]=1.0;
goalA.planning_scene_diff.robot_state.joint_state.position[2]=0.5;
goalA.planning_scene_diff.robot_state.joint_state.position[3]=1.0;
goalA.planning_scene_diff.robot_state.joint_state.position[4]=0.5;
goalA.planning_scene_diff.robot_state.joint_state.position[5]=0.0;
goalA.planning_scene_diff.robot_state.joint_state.position[6]=0.5;
if (nh.ok())
{
bool finished_within_time = false;
move_arm.sendGoal(goalA);
finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
if (!finished_within_time)
{
move_arm.cancelGoal();
ROS_INFO("Timed out achieving goal A");
}
else
{
actionlib::SimpleClientGoalState state = move_arm.getState();
bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
if(success)
ROS_INFO("Action finished: %s",state.toString().c_str());
else
ROS_INFO("Action failed: %s",state.toString().c_str());
}
}
as missing in electric tutorials for this, anyone know how to do? thanks for all!
enter code here