joint_control_pcan.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <aubo_msgs/JointPos.h>
00003 #include <std_msgs/Float32MultiArray.h>
00004 #include "jointcontrolapi.h"
00005 
00006 const char* CanDev = "/dev/pcan32";
00007 
00008 int can_int_flag = 0;
00009 
00010 void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
00011 {
00012     ROS_INFO("[%f,%f,%f,%f,%f,%f]",msg->data[0],msg->data[1],msg->data[2],msg->data[3],msg->data[4],msg->data[5]);
00013 
00014     double pos[6];
00015     pos[0] = msg->data[0];
00016     pos[1] = msg->data[1];
00017     pos[2] = msg->data[2];
00018     pos[3] = msg->data[3];
00019     pos[4] = msg->data[4];
00020     pos[5] = msg->data[5];
00021 
00022     //use can lib
00023     if(can_int_flag == 1)
00024     {
00025         set_joint_position(1,pos[0]);
00026         set_joint_position(2,pos[1]);
00027         set_joint_position(3,pos[2]);
00028         set_joint_position(4,pos[3]);
00029         set_joint_position(5,pos[4]);
00030         set_joint_position(6,pos[5]);
00031     }
00032 }
00033 
00034 int main(int argc, char **argv)
00035 {
00036     int ret;
00037     ros::init(argc, argv, "joint_control_pcan");
00038 
00039     std::string  level;
00040     int speed_level = 1;
00041 
00042     if (!(ros::param::get("~speed_level", level))) {
00043         if (argc > 1) {
00044            level = argv[1];
00045 
00046            if(!level.compare("-S1"))
00047            {
00048                 ROS_INFO("S1");
00049                 speed_level = 1;
00050            }
00051            else if(!level.compare("-S2"))
00052            {
00053                ROS_INFO("S2");
00054                 speed_level = 2;
00055            }
00056            else if(!level.compare("-S3"))
00057            {
00058                 ROS_INFO("S3");
00059                 speed_level = 3;
00060            }
00061            else
00062            {
00063                ROS_WARN("Wrong speed control command line parameter(-S1,-S2 or -S3)");
00064                exit(1);
00065            }
00066         }
00067     }
00068 
00069     ros::NodeHandle n;
00070     ros::Rate loop_rate(20);
00071 
00072 
00073     if(joint_control_init(CanDev) == 0)
00074     {
00075         ROS_INFO("can bus init!");
00076         can_int_flag = 1;
00077     }
00078 
00079     if(speed_level == 1)
00080     {
00081         set_joint_max_speed(1,1800);
00082         set_joint_max_speed(2,1800);
00083         set_joint_max_speed(3,1800);
00084         set_joint_max_speed(4,1800);
00085         set_joint_max_speed(5,1800);
00086         set_joint_max_speed(6,1800);
00087     }
00088     else if(speed_level == 2)
00089     {
00090         set_joint_max_speed(1,2300);
00091         set_joint_max_speed(2,2300);
00092         set_joint_max_speed(3,2300);
00093         set_joint_max_speed(4,2300);
00094         set_joint_max_speed(5,2300);
00095         set_joint_max_speed(6,2300);
00096     }
00097     else if(speed_level == 3)
00098     {
00099         set_joint_max_speed(1,2800);
00100         set_joint_max_speed(2,2800);
00101         set_joint_max_speed(3,2800);
00102         set_joint_max_speed(4,2800);
00103         set_joint_max_speed(5,2800);
00104         set_joint_max_speed(6,2800);
00105     }
00106 
00107     set_joint_max_acc(1,5000);
00108     set_joint_max_acc(2,5000);
00109     set_joint_max_acc(3,5000);
00110     set_joint_max_acc(4,5000);
00111     set_joint_max_acc(5,5000);
00112     set_joint_max_acc(6,5000);
00113 
00114     aubo_msgs::JointPos joint_pos;
00115 
00116 
00117 
00118     ros::Publisher state_pub = n.advertise<aubo_msgs::JointPos>("current_pos", 1);
00119 
00120     ros::Subscriber cmd_sub = n.subscribe("pcan_cmd", 1000 , chatterCallback);
00121 
00122    
00123     while(ros::ok())
00124     {
00125         if(can_int_flag == 1)
00126         {
00127             joint_pos.joint1 = read_joint_position(1);
00128             joint_pos.joint2 = read_joint_position(2);
00129             joint_pos.joint3 = read_joint_position(3);
00130             joint_pos.joint4 = read_joint_position(4);
00131             joint_pos.joint5 = read_joint_position(5);
00132             joint_pos.joint6 = read_joint_position(6);
00133 
00134             state_pub.publish(joint_pos);
00135         }
00136         loop_rate.sleep();
00137         ros::spinOnce();
00138     }
00139 
00140     return 0;
00141 }
00142 


aubo_control
Author(s): liuxin
autogenerated on Wed Sep 6 2017 03:06:39