#include "ros/ros.h"
#include "std_msgs/String.h"
#include <rmi_driver/commands.h>

#include <sstream>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;
  ros::Publisher cmd_pub = n.advertise<robot_movement_interface::CommandList>("command_list",10);
  ros::Rate loop_rate(2);

  while (ros::ok())
  {
    robot_movement_interface::Command cmd;
    robot_movement_interface::CommandList cmd_list;
    
    cmd = robot_movement_interface::Command();
    cmd.command_type = "PTP";
    cmd.pose_type = "JOINTS";
    cmd.velocity_type = "ROS";
    int num_joints = 7;
    for(int i = 0 ; i < 7; i++){
      cmd.pose.push_back(i*1.0);
      cmd.velocity.push_back(i*1.0);
    }
    
    //cmd = robot_movement_interface::Command();
    //cmd.command_type = "GET";
    //cmd.pose_type = "JOINT_POSITION";
    
    cmd_list = robot_movement_interface::CommandList();
    cmd_list.commands.push_back(cmd);

    printf("published command type: %s\n",cmd.command_type.c_str());
    cmd_pub.publish(cmd_list);

    ros::spinOnce();

    loop_rate.sleep();
  }


  return 0;
}