I have question about moveit goal state...

edit

Hi I'm newbie of moveit and I have hard time to achieve updating query goal state of planning request.

actually I followed these code

https://gist.github.com/garaemon/c8a6...

but I'm not familiar with python, So I changed these code in c++

here is my C++ code

int main(int argc, char **argv){

      ros::init(argc,argv,"my_robot");
      ros::AsyncSpinner spinner(1);
      spinner.start();

      ros::NodeHandle n;
      ros::Publisher joy_pub;
      joy_pub = n.advertise<geometry_msgs::PoseStamped>("/rviz/moveit/move_marker/goal_left_7",1);
      ros::Publisher reset_goal_state_pub;
      reset_goal_state_pub = n.advertise<std_msgs::Empty>("/rviz/moveit/update_goal_state",2);
      ros::Publisher plan_pub;
      plan_pub = n.advertise<std_msgs::Empty>("/rviz/moveit/plan",3);
      ros::Publisher execute_pub;
      execute_pub = n.advertise<std_msgs::Empty>("/rviz/moveit/execute",4);
      ros::Publisher selector_pub;
      selector_pub = n.advertise<std_msgs::String>("/rviz/moveit/select_planning_group",5);
      std_msgs::Empty empty_cmd_msg;

      ros::Subscriber joy_sub;
      geometry_msgs::PoseStamped pose;
      sleep(5.0);
      joy_pub.publish(pose);
      sleep(1.0);
      printf("Start\n");
      sleep(5);
      reset_goal_state_pub.publish(empty_cmd_msg);
      printf("reset goal state\n");
      std_msgs::String selector_msg;
      selector_msg.data = "left_manipulator";
      selector_pub.publish(selector_msg);
      printf("selector change\n");
      sleep(2);
      pose.header.frame_id = "/base_link";
      pose.header.stamp = ros::Time::now();
      pose.pose.position.x = 0.7;
      pose.pose.position.y = -0.3;
      pose.pose.position.z = 1.1;
      pose.pose.orientation.w = 1.0;
      joy_pub.publish(pose);
      printf("pose published\n");
      sleep(1);
      plan_pub.publish(empty_cmd_msg);
      printf("plan published\n");
      ros::waitForShutdown();
}

no error messages coming from moveit and my node but

in rviz robot's goal state does not changed and noting happed... is there any good solution of this?

my system is ubutntu 12.04 LTS and I installed ros hydro. and my robot has two armed robot.(each arm has 7 DOF

so I named goal_left_7....)

thank you for reading my queestion and waiting for answer as soon as possible...