cob_simulation_tester.cpp
Go to the documentation of this file.
00001 #include "ros/ros.h"
00002 #include <kdl/jntarray.hpp>
00003 #include <sensor_msgs/JointState.h>
00004 #include <brics_actuator/JointVelocities.h>
00005 #include <trajectory_msgs/JointTrajectory.h>
00006 #include <cob_srvs/Trigger.h>
00007 
00008 
00009 
00010 using namespace std;
00011 using namespace KDL;
00012 
00013 KDL::JntArray VirtualQ;
00014 KDL::JntArray q;
00015 KDL::JntArray q_last;
00016 ros::Time last;
00017 bool started = false;
00018 
00019 ros::Publisher arm_pub_;  //publish topic arm_controller/command
00020 ros::ServiceServer serv_reset;//service for resetting interface
00021 
00022 
00023 
00024 void sendVel(JntArray q_dot)
00025 {
00026         ros::Time now = ros::Time::now();
00027         double dt = now.toSec() - last.toSec();
00028         last = now;
00029         double horizon = 3.0*dt;
00030         //std::cout << dt << "\n";
00031 
00032         trajectory_msgs::JointTrajectory traj;
00033         traj.header.stamp = ros::Time::now()+ros::Duration(0.01);
00034         traj.joint_names.push_back("arm_1_joint");
00035         traj.joint_names.push_back("arm_2_joint");
00036         traj.joint_names.push_back("arm_3_joint");
00037         traj.joint_names.push_back("arm_4_joint");
00038         traj.joint_names.push_back("arm_5_joint");
00039         traj.joint_names.push_back("arm_6_joint");
00040         traj.joint_names.push_back("arm_7_joint");
00041 
00042         traj.points.resize(1);
00043         bool nonzero = false;
00044         for(int i = 0; i < 7; i++)
00045         {
00046                 if(fabs(q_dot(i)) >= 0.0)
00047                 {
00048                         traj.points[0].positions.push_back(VirtualQ(i) + q_dot(i)*horizon);
00049                         traj.points[0].velocities.push_back(q_dot(i));
00050                         VirtualQ(i) += q_dot(i)*dt;
00051                         nonzero = true;
00052                 }
00053         }
00054         traj.points[0].time_from_start = ros::Duration(horizon);
00055         if(nonzero)
00056                 arm_pub_.publish(traj);
00057 }
00058 
00059 JntArray parseJointStates(std::vector<std::string> names, std::vector<double> positions)
00060 {
00061         JntArray q_temp(7);
00062         int count = 0;
00063     bool parsed = false;
00064         for(unsigned int i = 0; i < names.size(); i++)
00065     {
00066                         if(strncmp(names[i].c_str(), "arm_", 4) == 0)
00067                         {
00068                                 q_temp(count) = positions[i];
00069                                 count++;
00070                                 parsed = true;
00071       }
00072     }
00073         if(!parsed)
00074                 return q_last;
00075         q_last = q_temp;
00076         //ROS_INFO("CurrentConfig: %f %f %f %f %f %f %f", q_temp(0), q_temp(1), q_temp(2), q_temp(3), q_temp(4), q_temp(5), q_temp(6));
00077         if(!started)
00078         {
00079                 //JntArray zero(7);
00080                 //sendVel(zero);
00081                 VirtualQ = q_temp;
00082                 started = true;
00083                 last = ros::Time::now();
00084 
00085                 ROS_INFO("Starting up controller with first configuration: %f %f %f", q_temp(0), q_temp(1), q_temp(2));
00086         }
00087         return q_temp;
00088 }
00089 
00090 bool resetCB(cob_srvs::Trigger::Request& request, cob_srvs::Trigger::Response& response)
00091 {
00092         started = false;
00093         return true;
00094 }
00095 
00096 
00097 void controllerStateCallback(const sensor_msgs::JointState::ConstPtr& msg)
00098 {
00099         std::vector<std::string> names = msg->name;
00100         std::vector<double> positions = msg->position;
00101         q = parseJointStates(names,positions);
00102 }
00103 
00104 void velocityCallback(const brics_actuator::JointVelocities::ConstPtr& msg)
00105 {
00106         KDL::JntArray q_dot;
00107         q_dot.resize(7);
00108         for(unsigned int j = 0; j < 7; j++)
00109         {
00110                 q_dot(j) = msg->velocities.at(j).value;
00111         }
00112         sendVel(q_dot);
00113 
00114 }
00115 
00116 
00117 int main(int argc, char **argv)
00118 {
00119         ros::init(argc, argv, "cob_simulation_tester");
00120         ros::NodeHandle n;
00121         arm_pub_ = n.advertise<trajectory_msgs::JointTrajectory>("/arm_controller/command",1);
00122         ros::Subscriber sub = n.subscribe("/joint_states", 1, controllerStateCallback);
00123         ros::Subscriber sub_vc = n.subscribe("/arm_controller/command_vel", 1, velocityCallback);
00124         serv_reset = n.advertiseService("reset_brics_interface", resetCB);
00125         ros::spin();
00126 
00127         return 0;
00128 }


cob_trajectory_controller
Author(s): Alexander Bubeck
autogenerated on Tue Mar 3 2015 15:12:41