simple_message: simple_message: utest.cpp Source File

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2011, Southwest Research Institute
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *
00010  *      * Redistributions of source code must retain the above copyright
00011  *      notice, this list of conditions and the following disclaimer.
00012  *      * Redistributions in binary form must reproduce the above copyright
00013  *      notice, this list of conditions and the following disclaimer in the
00014  *      documentation and/or other materials provided with the distribution.
00015  *      * Neither the name of the Southwest Research Institute, nor the names
00016  *      of its contributors may be used to endorse or promote products derived
00017  *      from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 
00032 #include "simple_message/simple_message.h"
00033 #include "simple_message/byte_array.h"
00034 #include "simple_message/shared_types.h"
00035 #include "simple_message/smpl_msg_connection.h"
00036 #include "simple_message/socket/udp_client.h"
00037 #include "simple_message/socket/udp_server.h"
00038 #include "simple_message/socket/tcp_client.h"
00039 #include "simple_message/socket/tcp_server.h"
00040 #include "simple_message/ping_message.h"
00041 #include "simple_message/ping_handler.h"
00042 #include "simple_message/messages/joint_message.h"
00043 #include "simple_message/joint_data.h"
00044 #include "simple_message/message_manager.h"
00045 #include "simple_message/simple_comms_fault_handler.h"
00046 #include "simple_message/joint_traj_pt.h"
00047 #include "simple_message/messages/joint_traj_pt_message.h"
00048 #include "simple_message/typed_message.h"
00049 #include "simple_message/joint_traj.h"
00050 
00051 #include <gtest/gtest.h>
00052 // Use pthread instead of boost::thread so we can cancel the TCP/UDP server
00053 // threads 
00054 //#include <boost/thread/thread.hpp>
00055 #include <pthread.h>
00056 
00057 using namespace industrial::simple_message;
00058 using namespace industrial::byte_array;
00059 using namespace industrial::shared_types;
00060 using namespace industrial::smpl_msg_connection;
00061 using namespace industrial::udp_socket;
00062 using namespace industrial::udp_client;
00063 using namespace industrial::udp_server;
00064 using namespace industrial::tcp_socket;
00065 using namespace industrial::tcp_client;
00066 using namespace industrial::tcp_server;
00067 using namespace industrial::ping_message;
00068 using namespace industrial::ping_handler;
00069 using namespace industrial::joint_data;
00070 using namespace industrial::joint_message;
00071 using namespace industrial::message_manager;
00072 using namespace industrial::simple_comms_fault_handler;
00073 using namespace industrial::joint_traj_pt;
00074 using namespace industrial::joint_traj_pt_message;
00075 using namespace industrial::typed_message;
00076 using namespace industrial::joint_traj;
00077 
00078 TEST(ByteArraySuite, init)
00079 {
00080 
00081   const shared_int SIZE = 100;
00082 
00083   ByteArray bytes;
00084   shared_int TOO_BIG = bytes.getMaxBufferSize()+1;
00085 
00086   char bigBuffer[TOO_BIG];
00087   char buffer[SIZE];
00088 
00089   // Valid byte arrays
00090   EXPECT_TRUE(bytes.init(&buffer[0], SIZE));
00091   EXPECT_EQ((shared_int)bytes.getBufferSize(), SIZE);
00092 
00093   // Invalid init (too big)
00094   // Invalid buffers
00095   EXPECT_FALSE(bytes.init(&bigBuffer[0], TOO_BIG));
00096 }
00097 
00098 TEST(ByteArraySuite, loading)
00099 {
00100   const shared_int SIZE = 100;
00101   char buffer[SIZE];
00102 
00103   ByteArray bytes;
00104   ByteArray empty;
00105 
00106   ASSERT_TRUE(bytes.init(&buffer[0], SIZE));
00107 
00108   shared_bool bIN = true, bOUT = false;
00109   shared_int iIN = 999, iOUT = 0;
00110   shared_real rIN = 9999.9999, rOUT = 0;
00111 
00112   // Boolean loading
00113   EXPECT_TRUE(bytes.load(bIN));
00114   EXPECT_EQ(bytes.getBufferSize(), SIZE+sizeof(shared_bool));
00115   EXPECT_TRUE(bytes.unload(bOUT));
00116   EXPECT_EQ((shared_int)bytes.getBufferSize(), SIZE);
00117   EXPECT_EQ(bOUT, bIN);
00118 
00119   // Integer loading
00120   EXPECT_TRUE(bytes.load(iIN));
00121   EXPECT_EQ(bytes.getBufferSize(), SIZE+sizeof(shared_int));
00122   EXPECT_TRUE(bytes.unload(iOUT));
00123   EXPECT_EQ((shared_int)bytes.getBufferSize(), SIZE);
00124   EXPECT_EQ(iOUT, iIN);
00125 
00126   // Real loading
00127   EXPECT_TRUE(bytes.load(rIN));
00128   EXPECT_EQ(bytes.getBufferSize(), SIZE+sizeof(shared_real));
00129   EXPECT_TRUE(bytes.unload(rOUT));
00130   EXPECT_EQ((shared_int)bytes.getBufferSize(), SIZE);
00131   EXPECT_EQ(rOUT, rIN);
00132 
00133   // Unloading a single member (down to an empty buffer size)
00134   EXPECT_TRUE(empty.load(bIN));
00135   EXPECT_EQ(empty.getBufferSize(), sizeof(shared_bool));
00136   EXPECT_TRUE(empty.unload(bOUT));
00137   EXPECT_EQ((int)empty.getBufferSize(), 0);
00138   EXPECT_EQ(bOUT, bIN);
00139 
00140   // Loading two members (unloading the first) and then checking the value of the second
00141   rOUT = 0.0;
00142   iOUT = 0;
00143   EXPECT_TRUE(empty.load(rIN));
00144   EXPECT_EQ(empty.getBufferSize(), sizeof(shared_real));
00145   EXPECT_TRUE(empty.load(iIN));
00146   EXPECT_EQ(empty.getBufferSize(), sizeof(shared_real)+sizeof(shared_int));
00147   EXPECT_TRUE(empty.unloadFront((void*)&rOUT, sizeof(shared_real)));
00148   EXPECT_EQ(rOUT, rIN);
00149   EXPECT_TRUE(empty.unload(iOUT));
00150   EXPECT_EQ((int)empty.getBufferSize(), 0);
00151   EXPECT_EQ(iOUT, iIN);
00152 }
00153 
00154 TEST(ByteArraySuite, copy)
00155 {
00156 
00157   const shared_int SIZE = 100;
00158   char buffer[SIZE];
00159 
00160   // Copy
00161   ByteArray copyFrom;
00162   ByteArray copyTo;
00163   ByteArray tooBig;
00164 
00165 
00166   shared_int TOO_BIG = tooBig.getMaxBufferSize()-1;
00167   char bigBuffer[TOO_BIG];
00168 
00169   EXPECT_TRUE(copyFrom.init(&buffer[0], SIZE));
00170   EXPECT_TRUE(copyTo.load(copyFrom));
00171   EXPECT_EQ((shared_int)copyTo.getBufferSize(), SIZE);
00172   EXPECT_TRUE(copyTo.load(copyFrom));
00173   EXPECT_EQ((shared_int)copyTo.getBufferSize(), 2*SIZE);
00174 
00175   // Copy too large
00176   EXPECT_TRUE(tooBig.init(&bigBuffer[0], TOO_BIG));
00177   EXPECT_FALSE(copyTo.load(tooBig));
00178   // A failed load should not change the buffer.
00179   EXPECT_EQ((shared_int)copyTo.getBufferSize(), 2*SIZE);
00180 }
00181 
00182 TEST(SimpleMessageSuite, init)
00183 {
00184   SimpleMessage msg;
00185   ByteArray bytes;
00186 
00187   // Valid messages
00188   EXPECT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::TOPIC, ReplyTypes::INVALID, bytes));
00189   EXPECT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST,ReplyTypes::INVALID, bytes));
00190   EXPECT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REPLY,ReplyTypes::SUCCESS, bytes));
00191   EXPECT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REPLY,ReplyTypes::FAILURE, bytes));
00192 
00193   // Unused command
00194   EXPECT_FALSE(msg.init(StandardMsgTypes::INVALID, CommTypes::INVALID,ReplyTypes::INVALID, bytes));
00195 
00196   // Service request with a reply
00197   EXPECT_FALSE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST,ReplyTypes::SUCCESS, bytes));
00198   EXPECT_FALSE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST,ReplyTypes::FAILURE, bytes));
00199 }
00200 
00201 TEST(PingMessageSuite, init)
00202 {
00203   PingMessage ping;
00204   SimpleMessage msg;
00205 
00206   EXPECT_FALSE(ping.init(msg));
00207   ping.init();
00208   EXPECT_EQ(StandardMsgTypes::PING, ping.getMessageType());
00209 
00210   ping = PingMessage();
00211   ASSERT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST, ReplyTypes::INVALID));
00212   EXPECT_TRUE(ping.init(msg));
00213   EXPECT_EQ(StandardMsgTypes::PING, ping.getMessageType());
00214 }
00215 
00216 TEST(PingMessageSuite, toMessage)
00217 {
00218   PingMessage ping;
00219   SimpleMessage msg;
00220 
00221   ping.init();
00222 
00223   ASSERT_TRUE(ping.toReply(msg, ReplyTypes::SUCCESS));
00224   EXPECT_EQ(StandardMsgTypes::PING, msg.getMessageType());
00225   EXPECT_EQ(CommTypes::SERVICE_REPLY, msg.getCommType());
00226   EXPECT_EQ(ReplyTypes::SUCCESS, msg.getReplyCode());
00227 
00228   ASSERT_TRUE(ping.toRequest(msg));
00229   EXPECT_EQ(StandardMsgTypes::PING, msg.getMessageType());
00230   EXPECT_EQ(CommTypes::SERVICE_REQUEST, msg.getCommType());
00231   EXPECT_EQ(ReplyTypes::INVALID, msg.getReplyCode());
00232 
00233   EXPECT_FALSE(ping.toTopic(msg));
00234 
00235 }
00236 
00237 TEST(PingHandlerSuite, init)
00238 {
00239   PingHandler handler;
00240   UdpClient udp;
00241 
00242   ASSERT_TRUE(handler.init(&udp));
00243   EXPECT_EQ(StandardMsgTypes::PING, handler.getMsgType());
00244 
00245   EXPECT_FALSE(handler.init(NULL));
00246 
00247 }
00248 
00249 TEST(MessageManagerSuite, init)
00250 {
00251   MessageManager manager;
00252   UdpClient udp;
00253 
00254   EXPECT_TRUE(manager.init(&udp));
00255   EXPECT_FALSE(manager.init(NULL));
00256 
00257 }
00258 
00259 TEST(MessageManagerSuite, addHandler)
00260 {
00261   MessageManager manager;
00262   UdpClient udp;
00263   PingHandler handler;
00264 
00265   EXPECT_EQ(0, (int)manager.getNumHandlers());
00266 
00267   ASSERT_TRUE(manager.init(&udp));
00268   EXPECT_EQ(1, (int)manager.getNumHandlers());
00269   EXPECT_FALSE(manager.add(NULL));
00270 
00271   ASSERT_TRUE(handler.init(&udp));
00272   EXPECT_FALSE(manager.add(&handler));
00273 }
00274 
00275 // wrapper around MessageManager::spin() that can be passed to
00276 // pthread_create()
00277 void*
00278 spinFunc(void* arg)
00279 {
00280   MessageManager* mgr = (MessageManager*)arg;
00281   mgr->spin();
00282   return NULL;
00283 }
00284 
00285 TEST(DISABLED_MessageManagerSuite, udp)
00286 {
00287   const int udpPort = 11000;
00288   char ipAddr[] = "127.0.0.1";
00289 
00290   UdpClient* udpClient = new UdpClient();
00291   UdpServer udpServer;
00292   SimpleMessage pingRequest, pingReply;
00293   MessageManager udpManager;
00294 
00295   ASSERT_TRUE(pingRequest.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST, ReplyTypes::INVALID));
00296 
00297   // UDP Socket testing
00298   // Construct server and start in a thread
00299   ASSERT_TRUE(udpServer.init(udpPort));
00300   ASSERT_TRUE(udpManager.init(&udpServer));
00301   //boost::thread udpSrvThrd(boost::bind(&MessageManager::spin, &udpManager));
00302   pthread_t udpSrvThrd;
00303   pthread_create(&udpSrvThrd, NULL, spinFunc, &udpManager);
00304 
00305   // Construct a client and try to ping the server
00306   ASSERT_TRUE(udpClient->init(&ipAddr[0], udpPort));
00307   ASSERT_TRUE(udpClient->makeConnect());
00308   ASSERT_TRUE(udpClient->sendMsg(pingRequest));
00309   ASSERT_TRUE(udpClient->receiveMsg(pingReply));
00310   ASSERT_TRUE(udpClient->sendAndReceiveMsg(pingRequest, pingReply));
00311 
00312   // Delete client and try to reconnect
00313   delete udpClient;
00314   udpClient = new UdpClient();
00315   ASSERT_TRUE(udpClient->init(&ipAddr[0], udpPort));
00316   ASSERT_TRUE(udpClient->makeConnect());
00317   ASSERT_TRUE(udpClient->sendAndReceiveMsg(pingRequest, pingReply));
00318 
00319   pthread_cancel(udpSrvThrd);
00320   pthread_join(udpSrvThrd, NULL);
00321 }
00322 
00323 TEST(MessageManagerSuite, tcp)
00324 {
00325   const int tcpPort = 11000;
00326   char ipAddr[] = "127.0.0.1";
00327 
00328   TcpClient* tcpClient = new TcpClient();
00329   TcpServer tcpServer;
00330   SimpleMessage pingRequest, pingReply;
00331   MessageManager tcpManager;
00332 
00333   ASSERT_TRUE(pingRequest.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST, ReplyTypes::INVALID));
00334 
00335   // TCP Socket testing
00336 
00337   // Construct server
00338   ASSERT_TRUE(tcpServer.init(tcpPort));
00339 
00340   // Construct a client
00341   ASSERT_TRUE(tcpClient->init(&ipAddr[0], tcpPort));
00342   ASSERT_TRUE(tcpClient->makeConnect());
00343 
00344   // Listen for client connection, init manager and start thread
00345   ASSERT_TRUE(tcpServer.makeConnect());
00346   ASSERT_TRUE(tcpManager.init(&tcpServer));
00347 
00348   // TODO: The message manager is not thread safe (threads are used for testing,
00349   // but running the message manager in a thread results in errors when the
00350   // underlying connection is deconstructed before the manager
00351   //boost::thread tcpSrvThrd(boost::bind(&MessageManager::spin, &tcpManager));
00352   pthread_t tcpSrvThrd;
00353   pthread_create(&tcpSrvThrd, NULL, spinFunc, &tcpManager);
00354 
00355   // Ping the server
00356   ASSERT_TRUE(tcpClient->sendMsg(pingRequest));
00357   ASSERT_TRUE(tcpClient->receiveMsg(pingReply));
00358   ASSERT_TRUE(tcpClient->sendAndReceiveMsg(pingRequest, pingReply));
00359 
00360   // Delete client and try to reconnect
00361 
00362   delete tcpClient;
00363   sleep(10); //Allow time for client to destruct and free up port
00364   tcpClient = new TcpClient();
00365   ASSERT_TRUE(tcpClient->init(&ipAddr[0], tcpPort));
00366   ASSERT_TRUE(tcpClient->makeConnect());
00367   ASSERT_TRUE(tcpClient->sendAndReceiveMsg(pingRequest, pingReply));
00368 
00369   pthread_cancel(tcpSrvThrd);
00370   pthread_join(tcpSrvThrd, NULL);
00371 }
00372 
00373 TEST(JointMessage, init)
00374 {
00375   JointData joint;
00376 
00377   joint.init();
00378   EXPECT_TRUE(joint.setJoint(0, 1.0));
00379   EXPECT_TRUE(joint.setJoint(1, 2.0));
00380   EXPECT_TRUE(joint.setJoint(2, 3.0));
00381   EXPECT_TRUE(joint.setJoint(3, 4.0));
00382   EXPECT_TRUE(joint.setJoint(4, 5.0));
00383   EXPECT_TRUE(joint.setJoint(5, 6.0));
00384   EXPECT_TRUE(joint.setJoint(6, 7.0));
00385   EXPECT_TRUE(joint.setJoint(7, 8.0));
00386   EXPECT_TRUE(joint.setJoint(8, 9.0));
00387   EXPECT_TRUE(joint.setJoint(9, 10.0));
00388 
00389   EXPECT_FALSE(joint.setJoint(10, 11.0));
00390 
00391 }
00392 
00393 TEST(JointMessage, equal)
00394 {
00395   JointData jointlhs, jointrhs;
00396 
00397   jointrhs.init();
00398   jointlhs.init();
00399   jointlhs.setJoint(0, -1.0);
00400   jointlhs.setJoint(9, 1.0);
00401 
00402   EXPECT_FALSE(jointlhs==jointrhs);
00403 
00404   jointrhs.setJoint(0, -1.0);
00405   jointrhs.setJoint(9, 1.0);
00406 
00407   EXPECT_TRUE(jointlhs==jointrhs);
00408 
00409 }
00410 
00411 TEST(JointMessage, toMessage)
00412 {
00413   JointData toMessage, fromMessage;
00414   JointMessage msg;
00415 
00416   toMessage.init();
00417   toMessage.setJoint(4, 44.44);
00418 
00419   msg.init(1, toMessage);
00420 
00421   fromMessage.copyFrom(msg.getJoints());
00422 
00423   EXPECT_TRUE(toMessage==fromMessage);
00424 
00425 }
00426 
00427 // Message passing routine, used to send and receive a typed message
00428 // Useful for checking the packing and unpacking of message data.
00429 void messagePassing(TypedMessage &send, TypedMessage &recv)
00430 {
00431         const int tcpPort = 11010;
00432           char ipAddr[] = "127.0.0.1";
00433 
00434           TcpClient tcpClient;
00435           TcpServer tcpServer;
00436           SimpleMessage msgSend, msgRecv;
00437 
00438           ASSERT_TRUE(send.toTopic(msgSend));
00439 
00440           // Construct server
00441 
00442           ASSERT_TRUE(tcpServer.init(tcpPort));
00443 
00444           // Construct a client
00445           ASSERT_TRUE(tcpClient.init(&ipAddr[0], tcpPort));
00446           ASSERT_TRUE(tcpClient.makeConnect());
00447 
00448           ASSERT_TRUE(tcpServer.makeConnect());
00449 
00450           ASSERT_TRUE(tcpClient.sendMsg(msgSend));
00451           ASSERT_TRUE(tcpServer.receiveMsg(msgRecv));
00452           ASSERT_TRUE(recv.init(msgRecv));
00453 }
00454 
00455 TEST(JointMessage, Comms)
00456 {
00457 
00458   JointMessage jointSend, jointRecv;
00459   JointData posSend, posRecv;
00460 
00461   posSend.init();
00462   posSend.setJoint(0,1.0);
00463   posSend.setJoint(1,2.0);
00464   posSend.setJoint(2,3.0);
00465   posSend.setJoint(3,4.0);
00466   posSend.setJoint(4,5.0);
00467   posSend.setJoint(5,6.0);
00468   posSend.setJoint(6,7.0);
00469   posSend.setJoint(7,8.0);
00470   posSend.setJoint(8,9.0);
00471   posSend.setJoint(9,10.0);
00472 
00473   jointSend.init(1, posSend);
00474 
00475   messagePassing(jointSend, jointRecv);
00476 
00477   posRecv.copyFrom(jointRecv.getJoints());
00478   ASSERT_TRUE(posRecv==posSend);
00479 }
00480 
00481 
00482 TEST(JointTrajPt, equal)
00483 {
00484         JointTrajPt lhs, rhs;
00485         JointData joint;
00486 
00487   joint.init();
00488   ASSERT_TRUE(joint.setJoint(0, 1.0));
00489   ASSERT_TRUE(joint.setJoint(1, 2.0));
00490   ASSERT_TRUE(joint.setJoint(2, 3.0));
00491   ASSERT_TRUE(joint.setJoint(3, 4.0));
00492   ASSERT_TRUE(joint.setJoint(4, 5.0));
00493   ASSERT_TRUE(joint.setJoint(5, 6.0));
00494   ASSERT_TRUE(joint.setJoint(6, 7.0));
00495   ASSERT_TRUE(joint.setJoint(7, 8.0));
00496   ASSERT_TRUE(joint.setJoint(8, 9.0));
00497   ASSERT_TRUE(joint.setJoint(9, 10.0));
00498 
00499   rhs.init(1.0, joint, 50.0);
00500   EXPECT_FALSE(lhs==rhs);
00501 
00502   lhs.init(0, joint, 0);
00503   EXPECT_FALSE(lhs==rhs);
00504 
00505   lhs.copyFrom(rhs);
00506   EXPECT_TRUE(lhs==rhs);
00507 
00508 }
00509 
00510 TEST(JointTrajPt, toMessage)
00511 {
00512         JointTrajPt toMessage, fromMessage;
00513         JointTrajPtMessage msg;
00514 
00515   toMessage.init();
00516   toMessage.setSequence(99);
00517   msg.init(toMessage);
00518 
00519   fromMessage.copyFrom(msg.point_);
00520 
00521   EXPECT_TRUE(toMessage==fromMessage);
00522 
00523 }
00524 
00525 
00526 TEST(JointTrajPt, Comms)
00527 {
00528 
00529         JointTrajPtMessage jointSend, jointRecv;
00530         JointData data;
00531         JointTrajPt posSend, posRecv;
00532 
00533         data.init();
00534         data.setJoint(0,1.0);
00535         data.setJoint(1,2.0);
00536         data.setJoint(2,3.0);
00537         data.setJoint(3,4.0);
00538         data.setJoint(4,5.0);
00539         data.setJoint(5,6.0);
00540         data.setJoint(6,7.0);
00541         data.setJoint(7,8.0);
00542         data.setJoint(8,9.0);
00543         data.setJoint(9,10.0);
00544         posSend.init(1, data, 99);
00545 
00546   jointSend.init(posSend);
00547 
00548   messagePassing(jointSend, jointRecv);
00549 
00550   posRecv.copyFrom(jointRecv.point_);
00551   ASSERT_TRUE(posRecv==posSend);
00552 }
00553 
00554 TEST(JointTraj, equal)
00555 {
00556         JointTraj lhs, rhs;
00557         JointData joint;
00558         JointTrajPt point;
00559 
00560   joint.init();
00561   ASSERT_TRUE(joint.setJoint(0, 1.0));
00562   ASSERT_TRUE(joint.setJoint(1, 2.0));
00563   ASSERT_TRUE(joint.setJoint(2, 3.0));
00564   ASSERT_TRUE(joint.setJoint(3, 4.0));
00565   ASSERT_TRUE(joint.setJoint(4, 5.0));
00566   ASSERT_TRUE(joint.setJoint(5, 6.0));
00567   ASSERT_TRUE(joint.setJoint(6, 7.0));
00568   ASSERT_TRUE(joint.setJoint(7, 8.0));
00569   ASSERT_TRUE(joint.setJoint(8, 9.0));
00570   ASSERT_TRUE(joint.setJoint(9, 10.0));
00571 
00572   point.init(1.0, joint, 50.0);
00573   rhs.addPoint(point);
00574   EXPECT_FALSE(lhs==rhs);
00575 
00576   lhs.addPoint(point);
00577   EXPECT_TRUE(lhs==rhs);
00578 
00579   lhs.addPoint(point);
00580   EXPECT_FALSE(lhs==rhs);
00581 
00582   lhs.copyFrom(rhs);
00583   EXPECT_TRUE(lhs==rhs);
00584 
00585 }
00586 
00587 
00588 // Run all the tests that were declared with TEST()
00589 int main(int argc, char **argv)
00590 {
00591   testing::InitGoogleTest(&argc, argv);
00592   return RUN_ALL_TESTS();
00593 }
00594