void AES25::processCanRxEvent(const fmMsgs::can::ConstPtr& can_rx_msg) { if (can_rx_msg->id == 0x180) { motorSpeed.byte[0] = can_rx_msg->data[0]; motorSpeed.byte[1] = can_rx_msg->data[1]; motorTorque.byte[0] = can_rx_msg->data[2]; motorTorque.byte[1] = can_rx_msg->data[3]; motorAngle.byte[0] = can_rx_msg->data[4]; motorAngle.byte[1] = can_rx_msg->data[5]; controllerInfo_[INDEX_MOTOR_FLAGS] = can_rx_msg->data[6]; controllerInfo_[INDEX_CAN_TIMEOUT_TICKS] = CONTROLLER_CAN_TIMEOUT_TICKS; ROS_DEBUG_THROTTLE_NAMED(1, "AES25 0x180", "Motorspeed: %d MotorTorque: %d Motorangle: %d MotorFlags: %x", motorSpeed.sInt, motorTorque.sInt,motorAngle.sInt,controllerInfo_[INDEX_MOTOR_FLAGS]); } else if (can_rx_msg->id == 0x280) { controllerInfo_[INDEX_MOTOR_ALARM] = can_rx_msg->data[0]; controllerInfo_[INDEX_MOTOR_TEMP] = can_rx_msg->data[1]; controllerInfo_[INDEX_INVERTER_TEMP] = can_rx_msg->data[2]; controllerInfo_[INDEX_INVERTER_VOLTAGE] = can_rx_msg->data[3]; controllerInfo_[INDEX_MOTOR_PROTOCOL_LO] = can_rx_msg->data[4]; controllerInfo_[INDEX_MOTOR_PROTOCOL_HI] = can_rx_msg->data[5]; controllerInfo_[INDEX_CAN_TIMEOUT_TICKS] = CONTROLLER_CAN_TIMEOUT_TICKS; ROS_DEBUG_COND_NAMED(controllerInfo_[INDEX_MOTOR_ALARM], "AES25 0x280", "MotorAlarm: %d MotorTemp: %d InverterTemp: %d InverterVoltage: %d MotorProtocolVersionLo: %d MotorProtocolVersionHi: %d", controllerInfo_[INDEX_MOTOR_ALARM], controllerInfo_[INDEX_MOTOR_TEMP], controllerInfo_[INDEX_INVERTER_TEMP], controllerInfo_[INDEX_INVERTER_VOLTAGE], controllerInfo_[INDEX_MOTOR_PROTOCOL_LO], controllerInfo_[INDEX_MOTOR_PROTOCOL_HI]); } else if (can_rx_msg->id == 0x700) { ROS_DEBUG_THROTTLE_NAMED(1, "AES25 0x700","Undescribed CANbus msg!"); } }
/** * @brief Send vision estimate transform to FCU position controller */ void send_vision_estimate(const ros::Time &stamp, const Eigen::Affine3d &tr) { /** * @warning Issue #60. * This now affects pose callbacks too. */ if (last_transform_stamp == stamp) { ROS_DEBUG_THROTTLE_NAMED(10, "vision_pose", "Vision: Same transform as last one, dropped."); return; } last_transform_stamp = stamp; auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation())); auto rpy = ftf::quaternion_to_rpy( ftf::transform_orientation_enu_ned(Eigen::Quaterniond(tr.rotation()))); vision_position_estimate(stamp.toNSec() / 1000, position, rpy); }
/** * Send vision estimate transform to FCU position controller */ void send_vision_transform(const tf::Transform &transform, const ros::Time &stamp) { // origin and RPY in ENU frame tf::Vector3 position = transform.getOrigin(); double roll, pitch, yaw; tf::Matrix3x3 orientation(transform.getBasis()); orientation.getRPY(roll, pitch, yaw); /* Issue #60. * Note: this now affects pose callbacks too, but i think its not big deal. */ if (last_transform_stamp == stamp) { ROS_DEBUG_THROTTLE_NAMED(10, "position", "Vision: Same transform as last one, dropped."); return; } last_transform_stamp = stamp; // TODO: check conversion. Issue #49. vision_position_estimate(stamp.toNSec() / 1000, position.y(), position.x(), -position.z(), roll, -pitch, -yaw); // ??? please check! }
void sendCommad(){ ros::Duration sleep_time(0.1); bool error = false; unsigned int num_req; while(nh_.ok() && ros::ok() && !error){ num_req = getRequestNum(); if (num_req > 0){ KukaRequest req = getRequest(); pop(); // TODO // Check option switch (req.opt_){ case STOP: // Send Stop error = kuka_.stop(); ROS_WARN_NAMED(name_, "Sending stop request to KUKA robot."); break; case PTP: // Send PTP command kuka_.setJointPosition(req.data_.ptp_goal); ROS_DEBUG_NAMED(name_, "Sending PTP request to KUKA robot."); break; case VEL: // Send PTP command kuka_.setOverrideSpeed(req.data_.vel); ROS_DEBUG_NAMED(name_, "Sending velocity override request to KUKA robot."); break; // case HOME: // // Send Home // for(unsigned int i=0; i<6; i++) req.data_.ptp_goal[i]=0.0*rad2deg+offset_[i]; // kuka_.setJointPosition(req.data_.ptp_goal); // ROS_DEBUG_NAMED(name_, "Sending HOME request to KUKA robot."); // break; default: ROS_ERROR_NAMED(name_, "Undefined option type."); } sleep_time = ros::Duration(0.1); } // Update joint state else if (num_req == 0) { ROS_DEBUG_NAMED(name_, "Update request to KUKA robot."); // Call KUKA update kuka_.update(); // Reset delta position and norm values double norm = 0.0; for (unsigned int i = 0; i < 6; ++i) position_d_[i] = joint_state_.position[i]; // Get joint position kuka_.getJointPosition(joint_state_.position); // Offset and rad convertion for (unsigned int i = 0; i < 6; ++i) { joint_state_.position[i] = (joint_state_.position[i] - offset_[i])*deg2rad; position_d_[i] -= joint_state_.position[i]; norm += position_d_[i]*position_d_[i]; } //norm = sqrt(norm); if ( norm < 0.01 ) sleep_time = ros::Duration(1); ROS_DEBUG_NAMED(name_, "Norma: %.2f", norm); // Publish joint state joint_state_.header.stamp = ros::Time::now(); joint_state_.header.seq++; joint_state_pub_.publish(joint_state_); } ROS_DEBUG_THROTTLE_NAMED(0.2, name_, "Current number of req %i", getRequestNum()); sleep_time.sleep(); } }
bool BaseController::update(const ros::Time now, const ros::Duration dt) { if (!initialized_) return false; // should never really hit this /* See if we have timed out and need to stop */ if (now - last_command_ >= timeout_) { ROS_DEBUG_THROTTLE_NAMED(5, "BaseController", "Command timed out."); desired_x_ = desired_r_ = 0.0; } /* Do velocity acceleration/limiting */ if (desired_x_ > last_sent_x_) { last_sent_x_ += max_acceleration_x_ * (now - last_update_).toSec(); if (last_sent_x_ > desired_x_) last_sent_x_ = desired_x_; } else { last_sent_x_ -= max_acceleration_x_ * (now - last_update_).toSec(); if (last_sent_x_ < desired_x_) last_sent_x_ = desired_x_; } if (desired_r_ > last_sent_r_) { last_sent_r_ += max_acceleration_r_ * (now - last_update_).toSec(); if (last_sent_r_ > desired_r_) last_sent_r_ = desired_r_; } else { last_sent_r_ -= max_acceleration_r_ * (now - last_update_).toSec(); if (last_sent_r_ < desired_r_) last_sent_r_ = desired_r_; } double dx = 0.0; double dr = 0.0; double left_dx = static_cast<double>((left_->getPosition() - left_last_position_)/radians_per_meter_); double right_dx = static_cast<double>((right_->getPosition() - right_last_position_)/radians_per_meter_); double left_vel = static_cast<double>(left_->getVelocity()/radians_per_meter_); double right_vel = static_cast<double>(right_->getVelocity()/radians_per_meter_); left_last_position_ = left_->getPosition(); right_last_position_ = right_->getPosition(); /* Calculate forward and angular differences */ double d = (left_dx+right_dx)/2.0; double th = (right_dx-left_dx)/track_width_; /* Calculate forward and angular velocities */ dx = (left_vel + right_vel)/2.0; dr = (right_vel - left_vel)/track_width_; /* Update store odometry */ odom_.pose.pose.position.x += d*cos(theta_); odom_.pose.pose.position.y += d*sin(theta_); theta_ += th; /* Actually set command */ if ((last_sent_x_ != 0.0) || (last_sent_r_ != 0.0) || (fabs(dx) > 0.05) || (fabs(dr) > 0.05)) { setCommand(last_sent_x_ - (last_sent_r_/2.0 * track_width_), last_sent_x_ + (last_sent_r_/2.0 * track_width_)); } /* Update odometry information. */ odom_.pose.pose.orientation.z = sin(theta_/2.0); odom_.pose.pose.orientation.w = cos(theta_/2.0); odom_.twist.twist.linear.x = dx; odom_.twist.twist.angular.z = dr; last_update_ = now; return true; }