sendmotion.cpp

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "iiwa_msgs/JointPosition.h"
#include "iiwa_msgs/JointQuantity.h"
#include "conversions.h"
#include "iiwa_ros.h"

/**
 * This is copied from the tutorial
 */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "sendmotion");
  ros::NodeHandle n;
  ros::Publisher sendmotion_pub = 
    n.advertise<iiwa_msgs::JointPosition>("iiwa/command/JointPosition", 1000);
  ros::Rate loop_rate(1);

  // The robot is slightly bent forward in this position
  iiwa_msgs::JointPosition my_joint_position;
  my_joint_position.position.a1 = 0.156;
  my_joint_position.position.a2 = 0.374;
  my_joint_position.position.a3 = 0.120;
  my_joint_position.position.a4 = -0.750;
  my_joint_position.position.a5 = -0.049;
  my_joint_position.position.a6 = 0.509;
  // my_joint_position.position.a7 = -1.571;
  my_joint_position.position.a7 = -1.3;
  
  int count = 0;
  while (ros::ok())
  {
    ROS_INFO("count %d", count);
    sendmotion_pub.publish(my_joint_position);

    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }

  return 0;
}