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;
}
