elektron_base_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <nav_msgs/Odometry.h>
00003 #include <geometry_msgs/Twist.h>
00004 #include <tf/transform_broadcaster.h>
00005 #include <ros/console.h>
00006 #include <pthread.h>
00007 #include "serialcomm/serialcomm.hpp"
00008 #include "nf/nfv2.h"
00009 #include "math.h"
00010 
00011 // wheel diameter in SI units [m]
00012 #define WHEEL_DIAM 0.1
00013 // axle length in SI units [m]
00014 #define AXLE_LENGTH 0.355
00015 
00016 // regulator rate in SI units [Hz]
00017 #define REGULATOR_RATE 100
00018 
00019 // maximum velocity, in internal units
00020 #define MAX_VEL 5500
00021 // number of encoder ticks per single wheel rotation
00022 #define ENC_TICKS 4000
00023 
00024 ros::Time cmd_time;
00025 
00026 NF_STRUCT_ComBuf        NFComBuf;
00027 SerialComm              *CommPort;
00028 uint8_t txBuf[256];
00029 uint8_t txCnt;
00030 uint8_t rxBuf[256];
00031 uint8_t rxCnt;
00032 uint8_t commandArray[256];
00033 uint8_t commandCnt;
00034 uint8_t rxCommandArray[256];
00035 uint8_t rxCommandCnt;
00036 
00037 void readDeviceVitalsTimerCallback(const ros::TimerEvent&) {
00038         commandArray[commandCnt++] = NF_COMMAND_ReadDeviceVitals;
00039 //      ROS_INFO("Added ReadDeviceVitals Command");
00040 }
00041 
00042 void twistCallback(const geometry_msgs::TwistConstPtr& msg) {
00043         double rotational_term = msg->angular.z * 0.335 / 2.0;
00044         double rvel = (msg->linear.x + rotational_term) * 4000 / M_PI / 0.314;
00045         double lvel = (msg->linear.x - rotational_term) * 4000 / M_PI / 0.314;
00046 
00047         if (rvel > MAX_VEL)
00048                 rvel = MAX_VEL;
00049         else if (rvel < -MAX_VEL)
00050                 rvel = -MAX_VEL;
00051 
00052         if (lvel > MAX_VEL)
00053                 lvel = MAX_VEL;
00054         else if (lvel < -MAX_VEL)
00055                 lvel = -MAX_VEL;
00056                 
00057         NFComBuf.SetDrivesSpeed.data[0] = lvel;
00058         NFComBuf.SetDrivesSpeed.data[1] = rvel;
00059         commandArray[commandCnt++] = NF_COMMAND_SetDrivesSpeed;
00060         ROS_INFO("Added SetDrivesSpeed Command %d, %d", NFComBuf.SetDrivesSpeed.data[0], NFComBuf.SetDrivesSpeed.data[1]);
00061         
00062         NFComBuf.SetDrivesMode.data[0] = NF_DrivesMode_SPEED;
00063         NFComBuf.SetDrivesMode.data[1] = NF_DrivesMode_SPEED;
00064         commandArray[commandCnt++] = NF_COMMAND_SetDrivesMode;
00065         ROS_INFO("Added SetDrivesMode Command NF_DrivesMode_SPEED, NF_DrivesMode_SPEED");
00066 }
00067 
00068 void *listener(void *p)
00069 {
00070         while(1) {
00071                 if(rxCnt == 255)
00072                         rxCnt = 0;
00073                 rxBuf[rxCnt] = CommPort->readOneByte();
00074                 //if(CommPort->read(&(rxBuf[rxCnt]), 1) > 0){
00075 //              ROS_INFO("RECEIVED %x", rxBuf[rxCnt]);
00076                 if(NF_Interpreter(&NFComBuf, rxBuf, &rxCnt, rxCommandArray, &rxCommandCnt) > 0){
00077 //                      ROS_INFO("Message Received!");
00078                 }
00079                 //}
00080         }
00081         pthread_exit(NULL);
00082 }
00083 
00084 int main(int argc, char** argv) {
00085         ros::init(argc, argv, "elektron_base_node");
00086         ros::NodeHandle n;
00087         ros::NodeHandle nh("~");
00088 
00089         bool publish_odom_tf;
00090         bool dump;
00091 
00092         double ls, rs;
00093 
00094         ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry> ("odom", 1);
00095         tf::TransformBroadcaster odom_broadcaster;
00096         ros::Subscriber twist_sub = n.subscribe("cmd_vel", 1, &twistCallback);
00097         ros::Rate loop_rate(100);
00098         std::string dev;
00099 
00100         if (!nh.getParam("device", dev)) {
00101                 dev = "/dev/ttyACM0";
00102         }
00103         if (!nh.getParam("publish_odom_tf", publish_odom_tf)) {
00104                 publish_odom_tf = false;
00105         }
00106         if (!nh.getParam("dump", dump)) {
00107                 dump = false;
00108         }
00109         if (!nh.getParam("lin_scale", ls)) {
00110                 ls = 1.0;
00111         }
00112         if (!nh.getParam("rot_scale", rs)) {
00113                 rs = 1.0;
00114         }
00115 
00116         nav_msgs::Odometry odom;
00117         odom.header.frame_id = "odom";
00118         odom.child_frame_id = "base_link";
00119 
00120         geometry_msgs::TransformStamped odom_trans;
00121         odom_trans.header.frame_id = "odom";
00122         odom_trans.child_frame_id = "base_link";
00123         
00124         // NFv2 Configuration
00125         NFv2_Config(&NFComBuf, NF_TerminalAddress);
00126     
00127         CommPort = new SerialComm(dev);
00128 
00129         if (! (CommPort->isConnected()) ) {
00130                 ROS_INFO("connection failed");
00131                 ROS_ERROR("Connection to device %s failed", dev.c_str());
00132                 return 0;
00133         }
00134         else {
00135                 ROS_INFO("connection ok");
00136                 
00137                 pthread_t listenerThread;
00138                 if(pthread_create(&listenerThread, NULL, &listener, NULL))
00139                         ROS_ERROR("Listener thread error");
00140                 else
00141                         ROS_INFO("Listener thread started");
00142                         
00143                 ros::Timer timer1 = n.createTimer(ros::Duration(1.0), readDeviceVitalsTimerCallback);
00144                 
00145                 while (ros::ok()) {
00146                         double x, y, th, xvel, thvel;
00147 
00148                         ros::Time current_time = ros::Time::now();
00149                         
00150                         // If communication with Elektron requested
00151                         if(commandCnt > 0) {
00152                                 txCnt = NF_MakeCommandFrame(&NFComBuf, txBuf, (const uint8_t*)commandArray, commandCnt, NF_RobotAddress);
00153                                 // Clear communication request
00154                                 commandCnt = 0;
00155                                 // Send command frame to Elektron
00156                                 CommPort->write(txBuf, txCnt);
00157                         }
00158 
00159 //                      p->updateOdometry();
00160 //                      p->getOdometry(x, y, th);
00161 //                      p->getVelocity(xvel, thvel);
00162                                                                 x = 0;
00163                                                                 y = 0;
00164                                                                 th = 0;
00165                                                                 xvel = 0;
00166                                                                 thvel = 0;
00167 
00168                         //since all odometry is 6DOF we'll need a quaternion created from yaw
00169                         geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
00170 
00171 
00172                         if (publish_odom_tf) {
00173                                 //first, we'll publish the transform over tf
00174                                 odom_trans.header.stamp = current_time;
00175 
00176                                 odom_trans.transform.translation.x = x;
00177                                 odom_trans.transform.translation.y = y;
00178                                 odom_trans.transform.translation.z = 0.0;
00179                                 odom_trans.transform.rotation = odom_quat;
00180 
00181                                 //send the transform
00182                                 odom_broadcaster.sendTransform(odom_trans);
00183                         }
00184 
00185                         //next, we'll publish the odometry message over ROS
00186                         odom.header.stamp = current_time;
00187 
00188                         //set the position
00189                         odom.pose.pose.position.x = x;
00190                         odom.pose.pose.position.y = y;
00191                         odom.pose.pose.position.z = 0.0;
00192                         odom.pose.pose.orientation = odom_quat;
00193 
00194                         odom.pose.covariance[0] = 0.00001;
00195                         odom.pose.covariance[7] = 0.00001;
00196                         odom.pose.covariance[14] = 10.0;
00197                         odom.pose.covariance[21] = 1.0;
00198                         odom.pose.covariance[28] = 1.0;
00199                         odom.pose.covariance[35] = thvel + 0.001;
00200 
00201                         //set the velocity
00202                         odom.child_frame_id = "base_link";
00203                         odom.twist.twist.linear.x = xvel;
00204                         odom.twist.twist.linear.y = 0.0;
00205                         odom.twist.twist.angular.z = thvel;
00206 
00207                         //publish the message
00208 //                      odom_pub.publish(odom);
00209 
00210                         ros::spinOnce();
00211                         loop_rate.sleep();
00212                 }
00213         } 
00214 
00215         return 0;
00216 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


elektron_base
Author(s): Konrad Banachowicz
autogenerated on Thu Nov 14 2013 11:57:03