youbot_2nd_arm_test.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002 * Copyright (c) 2011
00003 * Locomotec
00004 *
00005 * Author:
00006 * Sebastian Blumenthal
00007 *
00008 *
00009 * This software is published under a dual-license: GNU Lesser General Public
00010 * License LGPL 2.1 and BSD license. The dual-license implies that users of this
00011 * code may choose which terms they prefer.
00012 *
00013 * Redistribution and use in source and binary forms, with or without
00014 * modification, are permitted provided that the following conditions are met:
00015 *
00016 * * Redistributions of source code must retain the above copyright
00017 * notice, this list of conditions and the following disclaimer.
00018 * * Redistributions in binary form must reproduce the above copyright
00019 * notice, this list of conditions and the following disclaimer in the
00020 * documentation and/or other materials provided with the distribution.
00021 * * Neither the name of Locomotec nor the names of its
00022 * contributors may be used to endorse or promote products derived from
00023 * this software without specific prior written permission.
00024 *
00025 * This program is free software: you can redistribute it and/or modify
00026 * it under the terms of the GNU Lesser General Public License LGPL as
00027 * published by the Free Software Foundation, either version 2.1 of the
00028 * License, or (at your option) any later version or the BSD license.
00029 *
00030 * This program is distributed in the hope that it will be useful,
00031 * but WITHOUT ANY WARRANTY; without even the implied warranty of
00032 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00033 * GNU Lesser General Public License LGPL and the BSD license for more details.
00034 *
00035 * You should have received a copy of the GNU Lesser General Public
00036 * License LGPL and BSD license along with this program.
00037 *
00038 ******************************************************************************/
00039 
00040 #include <iostream>
00041 #include <assert.h>
00042 
00043 #include <ros/ros.h>
00044 #include <trajectory_msgs/JointTrajectory.h>
00045 #include <brics_actuator/CartesianWrench.h>
00046 
00047 #include <boost/units/io.hpp>
00048 
00049 #include <boost/units/systems/angle/degrees.hpp>
00050 #include <boost/units/conversion.hpp>
00051 
00052 #include <iostream>
00053 #include <assert.h>
00054 
00055 #include <ros/ros.h>
00056 #include <brics_actuator/JointPositions.h>
00057 
00058 #include <boost/units/systems/si/length.hpp>
00059 #include <boost/units/systems/si/plane_angle.hpp>
00060 #include <boost/units/io.hpp>
00061 
00062 #include <boost/units/systems/angle/degrees.hpp>
00063 #include <boost/units/conversion.hpp>
00064 
00065 using namespace std;
00066 
00067 int main(int argc, char **argv) {
00068 
00069         ros::init(argc, argv, "youbot_2nd_arm_test");
00070         ros::NodeHandle n;
00071         ros::Publisher armPositionsPublisher;
00072         ros::Publisher gripperPositionPublisher;
00073 
00074         armPositionsPublisher = n.advertise<brics_actuator::JointPositions > ("arm_2/arm_controller/position_command", 1);
00075         gripperPositionPublisher = n.advertise<brics_actuator::JointPositions > ("arm_2/gripper_controller/position_command", 1);
00076 
00077         ros::Rate rate(20); //Hz
00078         double readValue;
00079         static const int numberOfArmJoints = 5;
00080         static const int numberOfGripperJoints = 2;
00081         while (n.ok()) {
00082                 brics_actuator::JointPositions command;
00083                 vector <brics_actuator::JointValue> armJointPositions;
00084                 vector <brics_actuator::JointValue> gripperJointPositions;
00085 
00086                 armJointPositions.resize(numberOfArmJoints); //TODO:change that
00087                 gripperJointPositions.resize(numberOfGripperJoints);
00088 
00089                 std::stringstream jointName;
00090 
00091 
00092                 // ::io::base_unit_info <boost::units::si::angular_velocity>).name();
00093                 for (int i = 0; i < numberOfArmJoints; ++i) {
00094                         cout << "Please type in value for joint " << i + 1 << endl;
00095                         cin >> readValue;
00096 
00097                         jointName.str("");
00098                         jointName << "arm_2_joint_" << (i + 1);
00099 
00100                         armJointPositions[i].joint_uri = jointName.str();
00101                         armJointPositions[i].value = readValue;
00102 
00103                         armJointPositions[i].unit = boost::units::to_string(boost::units::si::radians);
00104                         cout << "Joint " << armJointPositions[i].joint_uri << " = " << armJointPositions[i].value << " " << armJointPositions[i].unit << endl;
00105 
00106                 };
00107 
00108                 cout << "Please type in value for a left jaw of the gripper " << endl;
00109                 cin >> readValue;
00110                 gripperJointPositions[0].joint_uri = "gripper_finger_joint_l";
00111                 gripperJointPositions[0].value = readValue;
00112                 gripperJointPositions[0].unit = boost::units::to_string(boost::units::si::meter);
00113 
00114                 cout << "Please type in value for a right jaw of the gripper " << endl;
00115                 cin >> readValue;
00116                 gripperJointPositions[1].joint_uri = "gripper_finger_joint_r";
00117                 gripperJointPositions[1].value = readValue;
00118                 gripperJointPositions[1].unit = boost::units::to_string(boost::units::si::meter);
00119 
00120                 cout << "sending command ..." << endl;
00121 
00122                 command.positions = armJointPositions;
00123                 armPositionsPublisher.publish(command);
00124 
00125                 command.positions = gripperJointPositions;
00126         gripperPositionPublisher.publish(command);
00127 
00128                 cout << "--------------------" << endl;
00129                 rate.sleep();
00130 
00131         }
00132 
00133         return 0;
00134 }
00135 
00136 /* EOF */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Defines


youbot_oodl
Author(s): Sebastian Blumenthal
autogenerated on Fri Jul 26 2013 12:00:42