$search
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 }