wge100_sim.cc
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009-2010, Willow Garage, Inc.
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
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 // Author: Blaise Gassend
00036 
00037 #include <sys/socket.h>
00038 #include <netinet/in.h>
00039 #include <sys/types.h>
00040 #include <arpa/inet.h>
00041 #include <string.h>
00042 #include <stdio.h>
00043 #include <stdlib.h>
00044 #include <unistd.h>
00045 #include <signal.h>
00046 #include <errno.h>
00047 #include <sched.h>
00048 
00049 #include <ros/console.h>
00050 #include <ros/time.h>
00051 
00052 #include <wge100_camera/ipcam_packet.h>
00053 #include <wge100_camera/host_netutil.h>
00054 #include <wge100_camera/wge100lib.h>
00055 #include <wge100_camera/mt9v.h>
00056 
00057 class WGE100Simulator
00058 {
00059 public:
00060   WGE100Simulator(uint32_t serial) : serial_no_(serial)
00061   {                      
00062     in_addr in_addr;
00063     in_addr.s_addr = 0;
00064     socket_ = wge100SocketCreate(&in_addr, WG_CAMCMD_PORT);
00065     if (socket_ == -1)
00066     {
00067       ROS_ERROR("Error creating/binding socket: %s", strerror(errno));
00068       return;
00069     }
00070     
00071     signal(SIGTERM, WGE100Simulator::setExiting);
00072     reset();
00073   }
00074 
00075   int run()
00076   {
00077     if (socket_ == -1)
00078       return -1;
00079   
00080     while (!exiting_)
00081     {
00082       ProcessCommands();
00083       SendData();
00084     }
00085 
00086     return 0;
00087   }
00088 
00089   ~WGE100Simulator()
00090   {
00091     if (socket_ != -1)
00092       close(socket_);
00093   }
00094 
00095 private:
00096   uint32_t serial_no_;
00097   int socket_;
00098   int running_;
00099   int width_;
00100   int height_;
00101   int frame_;
00102   //ros::Duration period_;
00103   ros::Time next_frame_time_;
00104   sockaddr_in data_addr_;
00105   static const int NUM_IMAGER_REGISTERS = 256;
00106   static const int IMAGER_REGISTER_UNDEFINED = 1;
00107   static const int IMAGER_REGISTER_UNSUPPORTED = 2;
00108   uint16_t imager_registers_[NUM_IMAGER_REGISTERS]; 
00109   uint8_t imager_register_flags_[NUM_IMAGER_REGISTERS];
00110 
00111   void reset()
00112   {
00113     running_ = 0;
00114     frame_ = 0;
00115     width_ = 640;
00116     height_ = 480;
00117     //period_ = ros::Duration(1/30.);
00118     next_frame_time_ = ros::Time(0);
00119     for (int i = 0; i < NUM_IMAGER_REGISTERS; i++)
00120     {
00121       imager_registers_[i] = 0;
00122       imager_register_flags_[i] |= IMAGER_REGISTER_UNDEFINED | IMAGER_REGISTER_UNSUPPORTED;
00123     }                
00124     uint8_t supported_registers[] = { 0x7F, 0x06, 0x05,  0x04,  0x03,      0 };
00125     uint16_t init_value[]         = { 0   , 0x2D, 0x5E, 0x2F0, 0x1E0, 0x1311 };
00126     for (unsigned int i = 0; i < sizeof(supported_registers) / sizeof(*supported_registers); i++)
00127     {
00128       imager_register_flags_[supported_registers[i]] &= ~(IMAGER_REGISTER_UNDEFINED | IMAGER_REGISTER_UNSUPPORTED);
00129       imager_registers_[supported_registers[i]] = init_value[i];
00130     }
00131                                                              
00132     uint8_t tolerated_registers[] = { 0x18, 0x15, 0x20, 0x21, 0x01, 0x02, 0x0D, 0x70, 0x71, 0x1C, 0xAF, 0x0B, 0x47, 0x48, 0x4C, 0xA5, 0x35, 0xBD };
00133 
00134     for (unsigned int i = 0; i < sizeof(tolerated_registers) / sizeof(*tolerated_registers); i++)
00135       imager_register_flags_[tolerated_registers[i]] &= ~IMAGER_REGISTER_UNSUPPORTED;
00136   }
00137       
00138   static inline uint8_t get_diag_test_pattern(int x, int y)
00139   {
00140     if ((x + 1) / 2 + y < 500)
00141       return 14 + x / 4;
00142     else
00143       return 0;
00144   }
00145 
00146   void SendData()
00147   {
00148     if (!running_)
00149     {
00150       next_frame_time_ = ros::Time(0);
00151       ros::Duration(0.01).sleep();
00152       return;
00153     }
00154 
00155     if (width_ != imager_registers_[0x04])
00156     {
00157       ROS_ERROR("Width mismatch %i reg: %i", width_, imager_registers_[0x04]);
00158       ros::Duration(0.01).sleep();
00159       return;
00160     }
00161     
00162     if (height_ != imager_registers_[0x03])
00163     {
00164       ROS_ERROR("Height mismatch %i reg: %i", height_, imager_registers_[0x03]);
00165       ros::Duration(0.01).sleep();
00166       return;
00167     }
00168     
00169     ros::Duration sleeptime = (next_frame_time_ - ros::Time::now());
00170     ros::Duration period = ros::Duration(((imager_registers_[0x03] + imager_registers_[0x06]) * (imager_registers_[0x04] + imager_registers_[0x05]) + 4) / 16e6);
00171     
00172     if (sleeptime < -period)
00173     {
00174       if (next_frame_time_ != ros::Time(0))
00175         ROS_WARN("Simulator lost lock on frame #%i", frame_);
00176       next_frame_time_ = ros::Time::now();
00177     }
00178     else
00179       sleeptime.sleep();
00180     next_frame_time_ += period;
00181     
00182     PacketVideoLine pvl;
00183     PacketEOF peof;
00184 
00185     peof.header.frame_number = pvl.header.frame_number = htonl(frame_);
00186     peof.header.horiz_resolution = pvl.header.horiz_resolution = htons(width_);
00187     peof.header.vert_resolution = pvl.header.vert_resolution = htons(height_);
00188   
00189     for (int y = 0; y < height_; y++)
00190     {
00191       if ((imager_registers_[0x7F] & 0x3C00) == 0x3800)
00192       {
00193         if (width_ > 320)
00194           for (int x = 0; x < width_; x++)
00195             pvl.data[x] = get_diag_test_pattern(x, y);
00196         else
00197           for (int x = 0; x < width_; x++)
00198             pvl.data[x] = (get_diag_test_pattern(2 * x, 2 * y) + get_diag_test_pattern(2 * x, 2 * y + 1)) / 2;
00199       }
00200       else
00201       {
00202         for (int x = 0; x < width_; x++)
00203           pvl.data[x] = x + y + frame_;
00204       }
00205 
00206       pvl.header.line_number = htons(y | (frame_ << 10));
00207       sendto(socket_, &pvl, sizeof(pvl.header) + width_, 0, 
00208           (struct sockaddr *) &data_addr_, sizeof(data_addr_));
00209       if (y == 0)
00210       {
00211         sched_yield(); // Make sure that first packet arrives with low jitter
00212         ROS_INFO("Sending frame #%i", frame_);
00213         fflush(stdout);
00214       }
00215     }
00216 
00217     peof.header.line_number = htons(IMAGER_LINENO_EOF);
00218     peof.i2c_valid = 0;
00219     peof.i2c[0] = 0;
00220     sendto(socket_, &peof, sizeof(peof), 0, 
00221         (struct sockaddr *) &data_addr_, sizeof(data_addr_));
00222     
00223     frame_ = (frame_ + 1) & 0xFFFF;
00224   }
00225 
00226   void ProcessCommands()
00227   {
00228     unsigned char buff[100];
00229 
00230     while (true)
00231     {
00232       ssize_t len = recv(socket_, buff, 100, MSG_DONTWAIT);
00233       if (len == -1)
00234         break;
00235 
00236       PacketGeneric *hdr = (PacketGeneric *) buff;
00237 
00238       if (len == 1 && buff[0] == 0xFF)
00239         continue; // This is the magic ping packet, just ignore it.
00240 
00241       if (ntohl(hdr->magic_no) != WG_MAGIC_NO ||
00242           ntohl(hdr->type) > PKT_MAX_ID)
00243       {
00244         ROS_INFO("Got a packet with a bad magic number or type."); \
00245         continue;
00246       }
00247 
00248 #define CAST_AND_CHK(type) \
00249 if (len != sizeof(type)) \
00250 { \
00251    ROS_INFO("Got a "#type" with incorrect length"); \
00252    continue;\
00253 } \
00254 if (false) ROS_INFO("Got a "#type""); \
00255 type *pkt = (type *) buff; 
00256 // End of this horrid macro that makes pkt appear by magic.
00257 
00258       switch (ntohl(hdr->type))
00259       {
00260         case PKTT_DISCOVER:
00261           {
00262             CAST_AND_CHK(PacketDiscover);
00263             sendAnnounce(&pkt->hdr);
00264             break;
00265           }
00266 
00267         case PKTT_CONFIGURE:
00268           {
00269             CAST_AND_CHK(PacketConfigure);
00270             if (pkt->product_id != htonl(CONFIG_PRODUCT_ID) ||
00271                 pkt->ser_no != htonl(serial_no_))
00272               break;
00273             running_ = false;
00274             sendAnnounce(&pkt->hdr);
00275             break;
00276           } 
00277     
00278         case PKTT_TRIGCTRL:
00279           {
00280             CAST_AND_CHK(PacketTrigControl);
00281             unsigned int new_mode = htons(pkt->trig_state);
00282             if (new_mode == 0 && !running_)
00283               sendStatus(&pkt->hdr, PKT_STATUST_OK, 0);
00284             else
00285             {
00286               ROS_ERROR("Tried to set to trigger mode %i.", new_mode);
00287               sendStatus(&pkt->hdr, PKT_STATUST_ERROR, PKT_ERROR_INVALID);
00288             }
00289             break;
00290           }
00291 
00292         case PKTT_IMGRMODE:
00293           {
00294             CAST_AND_CHK(PacketImagerMode);
00295             uint32_t mode = htonl(pkt->mode);
00296             if (mode >= 10 || running_)
00297             {
00298               sendStatus(&pkt->hdr, PKT_STATUST_ERROR, PKT_ERROR_INVALID);
00299               break;
00300             }
00301             //double fps[10] = {  15,12.5,  30,  25,  15,12.5,  60,  50,  30,  25 };
00302             //int widths[10] = { 752, 752, 640, 640, 640, 640, 320, 320, 320, 320 };
00303             //int heights[10] ={ 480, 480, 480, 480, 480, 480, 240, 240, 240, 240 };
00304             //period_ = ros::Duration(1/fps[mode]);
00305             width_ = MT9VModes[mode].width;
00306             height_ = MT9VModes[mode].height; 
00307             imager_registers_[0x03] = height_;
00308             imager_registers_[0x04] = width_;
00309             imager_registers_[0x05] = MT9VModes[mode].hblank;
00310             imager_registers_[0x06] = MT9VModes[mode].vblank;
00311             sendStatus(&pkt->hdr, PKT_STATUST_OK, 0);
00312           }
00313           break;
00314 
00315         case PKTT_IMGRSETRES:
00316           {
00317             CAST_AND_CHK(PacketImagerSetRes);
00318             unsigned int new_width = htons(pkt->horizontal);
00319             unsigned int new_height = htons(pkt->vertical);
00320             if (new_width > 752 || new_height > 640)
00321             {
00322               ROS_ERROR("Tried to set resolution to out of range %ix%i", new_width, new_height);
00323               sendStatus(&pkt->hdr, PKT_STATUST_ERROR, PKT_ERROR_INVALID);
00324               break;
00325             }
00326             //period_ = ros::Duration(1/10); // @todo fix this.
00327             width_ = new_width;
00328             height_ = new_height;
00329             sendStatus(&pkt->hdr, PKT_STATUST_OK, 0);
00330           }
00331           break;
00332 
00333         case PKTT_VIDSTART:
00334           {
00335             CAST_AND_CHK(PacketVidStart);
00336             if (running_)
00337               sendStatus(&pkt->hdr, PKT_STATUST_ERROR, PKT_ERROR_INVALID);
00338             else
00339             {
00340               sendStatus(&pkt->hdr, PKT_STATUST_OK, 0);
00341               bzero(&data_addr_, sizeof(data_addr_)); 
00342               data_addr_.sin_family = AF_INET; 
00343               data_addr_.sin_addr.s_addr = pkt->receiver.addr; 
00344               data_addr_.sin_port = pkt->receiver.port; 
00345             }
00346             running_ = true;
00347           }
00348           break;
00349 
00350         case PKTT_VIDSTOP:
00351           {
00352             CAST_AND_CHK(PacketVidStop);
00353             if (running_)
00354               sendStatus(&pkt->hdr, PKT_STATUST_OK, 0);
00355             else
00356               sendStatus(&pkt->hdr, PKT_STATUST_ERROR, PKT_ERROR_INVALID);
00357             running_ = false;
00358           }
00359           break;
00360 
00361         case PKTT_RESET:
00362           {
00363             reset();
00364           }
00365           break;
00366 
00367         case PKTT_SENSORRD:
00368           {
00369             CAST_AND_CHK(PacketSensorRequest);
00370             if (imager_register_flags_[pkt->address] & IMAGER_REGISTER_UNDEFINED)
00371               ROS_WARN("Reading uninitialized imager register 0x%02X.", pkt->address);
00372             if (imager_register_flags_[pkt->address] & IMAGER_REGISTER_UNSUPPORTED)
00373               ROS_WARN("Reading unsupported imager register 0x%02X.", pkt->address);
00374             sendSensorData(&pkt->hdr, pkt->address, imager_registers_[pkt->address]);
00375           }
00376           break;
00377 
00378         case PKTT_SENSORWR:
00379           {
00380             CAST_AND_CHK(PacketSensorData);
00381             if (imager_register_flags_[pkt->address] & IMAGER_REGISTER_UNSUPPORTED)
00382               ROS_ERROR("Writing unsupported imager register 0x%02X.", pkt->address);
00383             imager_register_flags_[pkt->address] &= ~IMAGER_REGISTER_UNDEFINED;
00384             imager_registers_[pkt->address] = ntohs(pkt->data);
00385             sendStatus(&pkt->hdr, PKT_STATUST_OK, 0);
00386           }
00387           break;
00388 
00389         case PKTT_FLASHREAD:
00390           {
00391             CAST_AND_CHK(PacketFlashRequest);
00392             sendFlashData(&pkt->hdr, pkt->address);
00393           }
00394           break;
00395 
00396         default:
00397           ROS_ERROR("Got an unknown message type: %i", ntohl(hdr->type));
00398           break;
00399       }
00400 #undef CAST_AND_CHK
00401     }
00402   }
00403   
00404 #define FILL_HDR(pkttype, pktid) \
00405 if (false) ROS_INFO("Sending a "#pkttype" packet."); \
00406 Packet##pkttype rsp; \
00407 rsp.hdr.magic_no = htonl(WG_MAGIC_NO); \
00408 strncpy(rsp.hdr.hrt, #pkttype, sizeof(rsp.hdr.hrt)); \
00409 rsp.hdr.type = htonl(pktid);
00410 // End of this horrid macro that makes resp appear by magic.
00411     
00412 #define SEND_RSP \
00413 sockaddr_in rsp_addr; \
00414 bzero(&rsp_addr, sizeof(rsp_addr)); \
00415 rsp_addr.sin_family = AF_INET; \
00416 rsp_addr.sin_addr.s_addr = hdr->reply_to.addr; \
00417 rsp_addr.sin_port = hdr->reply_to.port; \
00418 sendto(socket_, &rsp, sizeof(rsp), 0, \
00419     (struct sockaddr *) &rsp_addr, sizeof(rsp_addr)); 
00420 
00421   void sendAnnounce(PacketGeneric *hdr)
00422   {        
00423     FILL_HDR(Announce, PKTT_ANNOUNCE); 
00424     bzero(rsp.mac, sizeof(rsp.mac)); 
00425     rsp.product_id = htonl(CONFIG_PRODUCT_ID); 
00426     rsp.ser_no = htonl(serial_no_); 
00427     strncpy(rsp.product_name, "WGE100_camera_simulator.", sizeof(rsp.product_name)); 
00428     rsp.hw_version = htonl(0x2041); 
00429     rsp.fw_version = htonl(0x0112); 
00430     strncpy(rsp.camera_name, "WGE100_camera_simulator.", sizeof(rsp.camera_name)); 
00431     SEND_RSP;
00432   }
00433 
00434   void sendStatus(PacketGeneric *hdr, uint32_t type, uint32_t code)
00435   {        
00436     if (type != PKT_STATUST_OK)
00437       ROS_ERROR("Error Condition.");
00438     FILL_HDR(Status, PKTT_STATUS); 
00439     rsp.status_type = htonl(type);
00440     rsp.status_code = htonl(code);
00441     SEND_RSP;
00442   }
00443 
00444   void sendFlashData(PacketGeneric *hdr, uint32_t address)
00445   {
00446     FILL_HDR(FlashPayload, PKTT_FLASHDATA);
00447     rsp.address = address;
00448     bzero(rsp.data, sizeof(rsp.data));
00449     SEND_RSP;
00450   }
00451 
00452   void sendSensorData(PacketGeneric *hdr, uint8_t addr, uint16_t value)
00453   {        
00454     FILL_HDR(SensorData, PKTT_SENSORDATA); 
00455     rsp.address = addr;
00456     rsp.data = htons(value);
00457     SEND_RSP;
00458   }
00459 
00460 #undef FILL_HDR
00461 #undef SEND_RSP
00462   volatile static bool exiting_;
00463   
00464   static void setExiting(int i)
00465   {
00466     exiting_ = true;
00467   }
00468 };
00469 
00470 volatile bool WGE100Simulator::exiting_ = false;
00471 
00472 int main(int argc, char **argv)
00473 {
00474   ros::Time::init();
00475   WGE100Simulator wge100sim(12345);
00476   return wge100sim.run();
00477 }
00478 


wge100_camera
Author(s): Blaise Gassend, Patrick Mihelich, Eric MacIntosh, David Palchak
autogenerated on Mon Oct 6 2014 08:59:04