youbot_arm_test.cpp

Go to the documentation of this file.

/******************************************************************************
* Copyright (c) 2011
* Locomotec
*
* Author:
* Sebastian Blumenthal
*
*
* This software is published under a dual-license: GNU Lesser General Public
* License LGPL 2.1 and BSD license. The dual-license implies that users of this
* code may choose which terms they prefer.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Locomotec nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License LGPL as
* published by the Free Software Foundation, either version 2.1 of the
* License, or (at your option) any later version or the BSD license.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License LGPL and the BSD license for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License LGPL and BSD license along with this program.
*
******************************************************************************/

#include <iostream>
#include <assert.h>

#include "ros/ros.h"
#include "trajectory_msgs/JointTrajectory.h"
#include "brics_actuator/CartesianWrench.h"

#include <boost/units/io.hpp>

#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/conversion.hpp>

#include <iostream>
#include <assert.h>

#include "ros/ros.h"
#include "brics_actuator/JointPositions.h"

#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/io.hpp>

#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/conversion.hpp>

using namespace std;

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

        ros::init(argc, argv, "youbot_arm_test");
        ros::NodeHandle n;
        ros::Publisher armPositionsPublisher;
        ros::Publisher gripperPositionPublisher;

        armPositionsPublisher = n.advertise<brics_actuator::JointPositions > ("arm_1/arm_controller/position_command", 1);
        gripperPositionPublisher = n.advertise<brics_actuator::JointPositions > ("arm_1/gripper_controller/position_command", 1);

        ros::Rate rate(20); //Hz
        double readValue;
        static const int numberOfArmJoints = 5;
        static const int numberOfGripperJoints = 2;
        while (n.ok()) {
                brics_actuator::JointPositions command;
                vector <brics_actuator::JointValue> armJointPositions;
                vector <brics_actuator::JointValue> gripperJointPositions;

                armJointPositions.resize(numberOfArmJoints); //TODO:change that
                gripperJointPositions.resize(numberOfGripperJoints);

                std::stringstream jointName;


                // ::io::base_unit_info <boost::units::si::angular_velocity>).name();
                for (int i = 0; i < numberOfArmJoints; ++i) {
                        cout << "Please type in value for joint " << i + 1 << endl;
                        cin >> readValue;

                        jointName.str("");
                        jointName << "arm_joint_" << (i + 1);

                        armJointPositions[i].joint_uri = jointName.str();
                        armJointPositions[i].value = readValue;

                        armJointPositions[i].unit = boost::units::to_string(boost::units::si::radians);
                        cout << "Joint " << armJointPositions[i].joint_uri << " = " << armJointPositions[i].value << " " << armJointPositions[i].unit << endl;

                };

//              cout << "Please type in value for a left jaw of the gripper " << endl;
//              cin >> readValue;
//              gripperJointPositions[0].joint_uri = "gripper_finger_joint_l";
//              gripperJointPositions[0].value = readValue;
//              gripperJointPositions[0].unit = boost::units::to_string(boost::units::si::meter);

//              cout << "Please type in value for a right jaw of the gripper " << endl;
//              cin >> readValue;
//              gripperJointPositions[1].joint_uri = "gripper_finger_joint_r";
//              gripperJointPositions[1].value = readValue;
//              gripperJointPositions[1].unit = boost::units::to_string(boost::units::si::meter);

                cout << "sending command ..." << endl;

                command.positions = armJointPositions;
                armPositionsPublisher.publish(command);

//              command.positions = gripperJointPositions;
//              gripperPositionPublisher.publish(command);

                cout << "--------------------" << endl;
                ros::spinOnce();
                rate.sleep();

        }

        return 0;
}

/* EOF */