test_clientPOS.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <brics_actuator/JointPositions.h>              //brics
#include <brics_actuator/JointVelocities.h>             //brics

#include <sstream>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "test_client");

  ros::NodeHandle n;

  ros::Publisher chatter_pub = n.advertise<brics_actuator::JointPositions>("/cob_3d_mapping_demonstrator_controller/command_pos", 1);
  //  ros::Publisher chatter_pub = n.advertise<brics_actuator::JointVelocities>("/cob_3d_mapping_demonstrator_controller/command_vel", 1);

  ros::Rate loop_rate(20);

  float count = 0;

  while (ros::ok())
  {
    brics_actuator::JointPositions jp;
 //   brics_actuator::JointVelocities jv;
    brics_actuator::JointValue jointVal;
    jointVal.timeStamp = ros::Time::now();
    
    jointVal.joint_uri = "stepper";
    jointVal.unit = "rad";
    printf("Enter stepper target pos (rad): ");
    scanf("%f", &count);
    if(count == 9999)
        return 0;
        
    jointVal.value = (double)count;

    jp.positions.push_back(jointVal);

    jointVal.joint_uri = "servo";
    jointVal.unit = "rad";
    printf("Enter servo target pos (rad): ");
    scanf("%f", &count);
    jointVal.value = (double)count;

    jp.positions.push_back(jointVal);

        chatter_pub.publish(jp);


  //  chatter_pub.publish(jv);

    ros::spinOnce();

//    loop_rate.sleep();
  }
  return 0;
}