Exemple #1
0
int mwFileTransfer_close(struct mwFileTransfer *ft, guint32 code) {
  struct mwServiceFileTransfer *srvc;
  struct mwFileTransferHandler *handler;
  int ret = 0;

  g_return_val_if_fail(ft != NULL, -1);

  if(mwFileTransfer_isOpen(ft))
    ft_state(ft, mwFileTransfer_CANCEL_LOCAL);

  if(ft->channel) {
    ret = mwChannel_destroy(ft->channel, code, NULL);
    ft->channel = NULL;
  }

  srvc = ft->service;
  g_return_val_if_fail(srvc != NULL, ret);

  handler = srvc->handler;
  g_return_val_if_fail(handler != NULL, ret);

  if(handler->ft_closed)
    handler->ft_closed(ft, code);

  return ret;
}
Exemple #2
0
int mwFileTransfer_accept(struct mwFileTransfer *ft) {
  struct mwServiceFileTransfer *srvc;
  struct mwFileTransferHandler *handler;
  int ret;

  g_return_val_if_fail(ft != NULL, -1);
  g_return_val_if_fail(ft->channel != NULL, -1);
  g_return_val_if_fail(mwFileTransfer_isPending(ft), -1);
  g_return_val_if_fail(mwChannel_isIncoming(ft->channel), -1);
  g_return_val_if_fail(mwChannel_isState(ft->channel, mwChannel_WAIT), -1);

  g_return_val_if_fail(ft->service != NULL, -1);
  srvc = ft->service;

  g_return_val_if_fail(srvc->handler != NULL, -1);
  handler = srvc->handler;

  ret = mwChannel_accept(ft->channel);

  if(ret) {
    mwFileTransfer_close(ft, ERR_FAILURE);

  } else {
    ft_state(ft, mwFileTransfer_OPEN);
    if(handler->ft_opened)
      handler->ft_opened(ft);
  }

  return ret;
}
Exemple #3
0
static void recv_channelCreate(struct mwServiceFileTransfer *srvc,
			       struct mwChannel *chan,
			       struct mwMsgChannelCreate *msg) {

  struct mwFileTransferHandler *handler;
  struct mwGetBuffer *b;

  char *fnm, *txt;
  guint32 size, junk;
  gboolean b_err;

  g_return_if_fail(srvc->handler != NULL);
  handler = srvc->handler;
  
  b = mwGetBuffer_wrap(&msg->addtl);

  guint32_get(b, &junk); /* unknown */
  mwString_get(b, &fnm); /* offered filename */
  mwString_get(b, &txt); /* offering message */
  guint32_get(b, &size); /* size of offered file */
  /// Miranda NG adaptation - start - http://www.lilotux.net/~mikael/pub/meanwhile/ft_fix.diff
  /* guint32_get(b, &junk); */ /* unknown */
  /// Miranda NG adaptation - end
  /* and we just skip an unknown guint16 at the end */

  b_err = mwGetBuffer_error(b);
  mwGetBuffer_free(b);

  if(b_err) {
    g_warning("bad/malformed addtl in File Transfer service");
    mwChannel_destroy(chan, ERR_FAILURE, NULL);

  } else {
    struct mwIdBlock idb;
    struct mwFileTransfer *ft;

    login_into_id(&idb, mwChannel_getUser(chan));
    ft = mwFileTransfer_new(srvc, &idb, txt, fnm, size);
    ft->channel = chan;
    ft_state(ft, mwFileTransfer_PENDING);

    mwChannel_setServiceData(chan, ft, NULL);

    if(handler->ft_offered)
      handler->ft_offered(ft);
  }

  g_free(fnm);
  g_free(txt);
}
Exemple #4
0
int mwFileTransfer_offer(struct mwFileTransfer *ft) {
  struct mwServiceFileTransfer *srvc;
  struct mwFileTransferHandler *handler;

  g_return_val_if_fail(ft != NULL, -1);
  g_return_val_if_fail(ft->channel == NULL, -1);
  g_return_val_if_fail(mwFileTransfer_isNew(ft), -1);

  g_return_val_if_fail(ft->service != NULL, -1);
  srvc = ft->service;

  g_return_val_if_fail(srvc->handler != NULL, -1);
  handler = srvc->handler;

  ft_create_chan(ft);
  if(ft->channel) {
    ft_state(ft, mwFileTransfer_PENDING);
  } else {
    ft_state(ft, mwFileTransfer_ERROR);
    mwFileTransfer_close(ft, ERR_FAILURE);
  }

  return 0;
}
Exemple #5
0
static void recv_RECEIVED(struct mwFileTransfer *ft,
			  struct mwOpaque *data) {

  struct mwServiceFileTransfer *srvc;
  struct mwFileTransferHandler *handler;
  
  srvc = ft->service;
  handler = srvc->handler;

  if(! ft->remaining)
    ft_state(ft, mwFileTransfer_DONE);

  if(handler->ft_ack)
    handler->ft_ack(ft);

  if(! ft->remaining)
    mwFileTransfer_close(ft, mwFileTransfer_SUCCESS);
}
Exemple #6
0
static void recv_channelAccept(struct mwServiceFileTransfer *srvc,
			       struct mwChannel *chan,
			       struct mwMsgChannelAccept *msg) {

  struct mwFileTransferHandler *handler;
  struct mwFileTransfer *ft;

  g_return_if_fail(srvc->handler != NULL);
  handler = srvc->handler;

  ft = mwChannel_getServiceData(chan);
  g_return_if_fail(ft != NULL);

  ft_state(ft, mwFileTransfer_OPEN);

  if(handler->ft_opened)
    handler->ft_opened(ft);
}
Exemple #7
0
static void recv_channelDestroy(struct mwServiceFileTransfer *srvc,
				struct mwChannel *chan,
				struct mwMsgChannelDestroy *msg) {

  struct mwFileTransferHandler *handler;
  struct mwFileTransfer *ft;
  guint32 code;

  code = msg->reason;

  g_return_if_fail(srvc->handler != NULL);
  handler = srvc->handler;

  ft = mwChannel_getServiceData(chan);
  g_return_if_fail(ft != NULL);

  ft->channel = NULL;

  if(! mwFileTransfer_isDone(ft))
    ft_state(ft, mwFileTransfer_CANCEL_REMOTE);

  mwFileTransfer_close(ft, code);
}
Exemple #8
0
struct mwFileTransfer *
mwFileTransfer_new(struct mwServiceFileTransfer *srvc,
		   const struct mwIdBlock *who, const char *msg,
		   const char *filename, guint32 filesize) {
  
  struct mwFileTransfer *ft;

  g_return_val_if_fail(srvc != NULL, NULL);
  g_return_val_if_fail(who != NULL, NULL);
  
  ft = g_new0(struct mwFileTransfer, 1);
  ft->service = srvc;
  mwIdBlock_clone(&ft->who, who);
  ft->filename = g_strdup(filename);
  ft->message = g_strdup(msg);
  ft->size = ft->remaining = filesize;

  ft_state(ft, mwFileTransfer_NEW);

  /* stick a reference in the service */
  srvc->transfers = g_list_prepend(srvc->transfers, ft);

  return ft;
}
Exemple #9
0
static void recv_TRANSFER(struct mwFileTransfer *ft,
			  struct mwOpaque *data) {

  struct mwServiceFileTransfer *srvc;
  struct mwFileTransferHandler *handler;
  
  srvc = ft->service;
  handler = srvc->handler;

  g_return_if_fail(mwFileTransfer_isOpen(ft));

  if(data->len > ft->remaining) {
    /* @todo handle error */

  } else {
    ft->remaining -= data->len;

    if(! ft->remaining)
      ft_state(ft, mwFileTransfer_DONE);
    
    if(handler->ft_recv)
      handler->ft_recv(ft, data);
  }
}
Exemple #10
0
/*!
 * \brief Unpack force/torque ADC samples from realtime data.
 *
 * \return True, if there are no problems, false if there is something wrong with the data. 
 */
bool WG06::unpackFT(WG06StatusWithAccelAndFT *status, WG06StatusWithAccelAndFT *last_status)
{  
  pr2_hardware_interface::ForceTorqueState &ft_state(force_torque_.state_);

  ros::Time current_time(ros::Time::now());

  // Fill in raw analog output with most recent data sample, (might become deprecated?)
  {
    ft_raw_analog_in_.state_.state_.resize(6);
    const FTDataSample &sample(status->ft_samples_[0]);
    for (unsigned i=0; i<6; ++i)
    {
      int raw_data = sample.data_[i];
      ft_raw_analog_in_.state_.state_[i] = double(raw_data);
    }
  }

  unsigned new_samples = (unsigned(status->ft_sample_count_) - unsigned(last_status->ft_sample_count_)) & 0xFF;
  ft_sample_count_ += new_samples;
  int missed_samples = std::max(int(0), int(new_samples) - 4);
  ft_missed_samples_ += missed_samples;
  unsigned usable_samples = min(new_samples, MAX_FT_SAMPLES); 

  // Also, if number of new_samples is ever 0, then there is also an error
  if (usable_samples == 0)
  {
    ft_sampling_rate_error_ = true;
  }

  // Make room in data structure for more f/t samples
  ft_state.samples_.resize(usable_samples);

  // If any f/t channel is overload or the sampling rate is bad, there is an error.
  ft_state.good_ = ( (!ft_sampling_rate_error_) && 
                     (ft_overload_flags_ == 0) && 
                     (!ft_disconnected_) && 
                     (!ft_vhalf_error_) );

  for (unsigned sample_index=0; sample_index<usable_samples; ++sample_index)
  {
    // samples are stored in status data, so that newest sample is at index 0.
    // this is the reverse of the order data is stored in hardware_interface::ForceTorque buffer.
    unsigned status_sample_index = usable_samples-sample_index-1;
    const FTDataSample &sample(status->ft_samples_[status_sample_index]); 
    geometry_msgs::Wrench &wrench(ft_state.samples_[sample_index]);
    convertFTDataSampleToWrench(sample, wrench);
  }

  // Put newest sample into analog vector for controllers (deprecated)
  if (usable_samples > 0)
  {
    const geometry_msgs::Wrench &wrench(ft_state.samples_[usable_samples-1]);
    ft_analog_in_.state_.state_[0] = wrench.force.x;
    ft_analog_in_.state_.state_[1] = wrench.force.y;
    ft_analog_in_.state_.state_[2] = wrench.force.z;
    ft_analog_in_.state_.state_[3] = wrench.torque.x;
    ft_analog_in_.state_.state_[4] = wrench.torque.y;
    ft_analog_in_.state_.state_[5] = wrench.torque.z;
  }

  // Put all new samples in buffer and publish it
  if ((raw_ft_publisher_ != NULL) && (raw_ft_publisher_->trylock()))
  {
    raw_ft_publisher_->msg_.samples.resize(usable_samples);
    raw_ft_publisher_->msg_.sample_count = ft_sample_count_;
    raw_ft_publisher_->msg_.missed_samples = ft_missed_samples_;
    for (unsigned sample_num=0; sample_num<usable_samples; ++sample_num)
    {
      // put data into message so oldest data is first element
      const FTDataSample &sample(status->ft_samples_[sample_num]);
      ethercat_hardware::RawFTDataSample &msg_sample(raw_ft_publisher_->msg_.samples[usable_samples-sample_num-1]);
      msg_sample.sample_count = ft_sample_count_ - sample_num;
      msg_sample.data.resize(NUM_FT_CHANNELS);
      for (unsigned ch_num=0; ch_num<NUM_FT_CHANNELS; ++ch_num)
      {
        msg_sample.data[ch_num] = sample.data_[ch_num];
      }
      msg_sample.vhalf = sample.vhalf_;
    }
    raw_ft_publisher_->msg_.sample_count = ft_sample_count_;
    raw_ft_publisher_->unlockAndPublish();
  }

  // Put newest sample in realtime publisher
  if ( (usable_samples > 0) && (ft_publisher_ != NULL) && (ft_publisher_->trylock()) )
  {
    ft_publisher_->msg_.header.stamp = current_time;
    ft_publisher_->msg_.wrench = ft_state.samples_[usable_samples-1];
    ft_publisher_->unlockAndPublish();
  }

  // If this returns false, it will cause motors to halt.
  // Return "good_" state of sensor, unless halt_on_error is false. 
  return ft_state.good_ || !force_torque_.command_.halt_on_error_;
}