move_group_interface unable to see change in joint positions rviz

edit

Hi,

I am using move_group_interface and trying to visualize simple motion plans within rviz. When I specify a joint space goal, I record the position of each joints and print it out on the terminal before making a plan to that goal. Here is the code.

void SimpleMotionPlanning::goToJointState()
{
  robot_state::RobotState current_state = *move_group.getCurrentState();
  std::vector<double> joint_positions;
  joint_model_group = current_state.getJointModelGroup(PLANNING_GROUP);
  current_state.copyJointGroupPositions(joint_model_group, joint_positions);

  ROS_INFO("Before the execution of the motion ..... ");
  for (int i=0; i<joint_positions.size(); i++) {
    ROS_INFO_STREAM("joint_" << i <<": " << joint_positions[i]);
  }

  //joint_positions = move_group.getCurrentJointValues();

  // changing joint positions

  double refVals [] = {-1.8263219632699421, -1.7319098497843228,  1.7991931614989278, -1.6389153321983159, -1.5723347175650684, 2.8868157860256334};

  // vals in degrees approximate
  std::vector<double> newJointVals;

  for (int i=0; i<6; i++) {
    newJointVals.push_back(refVals[i]);
  }


  move_group.setJointValueTarget(newJointVals);
  bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

 // sleep(10.0);

  if (!success)
    throw std::runtime_error("No plan found");


   display_trajectory.trajectory_start = my_plan.start_state_;
   display_trajectory.trajectory.push_back(my_plan.trajectory_);

   display_pub.publish(display_trajectory);

 // sleep(10.0);
   //  move_group.move(); //blocking
  move_group.execute(my_plan);
}

Later on, when this motion has finished executing through a call smp.goToJointState(); (as I can see in rviz, robotic arm making a motion). I try to record the joint positions (in a different function and using the same methods), to verify that these are indeed the ones specified here double refVals [] = {-1.8263219632699421, -1.7319098497843228, 1.7991931614989278, -1.6389153321983159, -1.5723347175650684, 2.8868157860256334};

However, I receive all joint positions as 0 before and after the motion executions.

Before the execution of the motion ..... 
[ INFO] [1580386640.373732982]: joint_0: 0
[ INFO] [1580386640.373802724]: joint_1: 0
[ INFO] [1580386640.373865610]: joint_2: 0
[ INFO] [1580386640.373915428]: joint_3: 0
[ INFO] [1580386640.373964082]: joint_4: 0
[ INFO] [1580386640.374003310]: joint_5: 0

[ INFO] [1580386640.572283835]: After the execution of the motion ..... 
[ INFO] [1580386640.572328683]: joint_0: 0
[ INFO] [1580386640.572347079]: joint_1: 0
[ INFO] [1580386640.572360718]: joint_2: 0
[ INFO] [1580386640.572373839]: joint_3: 0
[ INFO] [1580386640.572387054]: joint_4: 0
[ INFO] [1580386640.572401063]: joint_5: 0

Can someone help me.

Here are contents of my launch file.

<launch>

  <include file="$(find ur5_moveit_path_planner)/launch/planning_context.launch" >
    <arg name="load_robot_description" value="true" />
  </include>

  <node
    name="joint_state_publisher"
    pkg="joint_state_publisher"
    type="joint_state_publisher">
    <param name="/use_gui" value="false"/>
    <rosparam param="/source_list">[/joint_states]</rosparam>
  </node>

  <node
    name="robot_state_publisher"
    pkg="robot_state_publisher"
    type="state_publisher"
  />

  <include file="$(find ur5_moveit_path_planner)/launch/moveit_rviz.launch"/>

  <include file="$(find ur5_moveit_path_planner)/launch/move_group.launch">
    <arg name="publish_monitored_planning_scene" value="true" />
    <arg name="allow_trajectory_execution" value="true" />
  </include>



  <!--
        including our own planner
  -->

  <arg
    name="option"
    default="1"
  />

  <node
    name="ur5"
    pkg="ur5_moveit_path_planner"
    type="ur5"
    respawn="false"
    output="screen"
    args="$(arg option)"
  />


</launch>

thanks, zahid