void timer_callback(const ros::TimerEvent &) {
if (!c3trajectory) return;
ros::Time now = ros::Time::now();
if (actionserver.isPreemptRequested()) {
current_waypoint = c3trajectory->getCurrentPoint();
current_waypoint.r.qdot = subjugator::Vector6d::Zero(); // zero velocities
current_waypoint_t = now;
// don't try to make output c3 continuous when cancelled - instead stop as quickly as possible
c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
c3trajectory_t = now;
}
if (actionserver.isNewGoalAvailable()) {
boost::shared_ptr<const uf_common::MoveToGoal> goal = actionserver.acceptNewGoal();
current_waypoint = subjugator::C3Trajectory::Waypoint(
Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist), goal->speed,
!goal->uncoordinated);
current_waypoint_t = now; // goal->header.stamp;
this->linear_tolerance = goal->linear_tolerance;
this->angular_tolerance = goal->angular_tolerance;
c3trajectory_t = now;
}
while ((c3trajectory_t + traj_dt < now) and ros::ok()) {
// ROS_INFO("Acting");
c3trajectory->update(traj_dt.toSec(), current_waypoint,
(c3trajectory_t - current_waypoint_t).toSec());
c3trajectory_t += traj_dt;
}
PoseTwistStamped msg;
msg.header.stamp = c3trajectory_t;
msg.header.frame_id = fixed_frame;
msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint());
trajectory_pub.publish(msg);
PoseStamped posemsg;
posemsg.header.stamp = c3trajectory_t;
posemsg.header.frame_id = fixed_frame;
posemsg.pose = Pose_from_Waypoint(current_waypoint);
waypoint_pose_pub.publish(posemsg);
if (actionserver.isActive() &&
c3trajectory->getCurrentPoint().is_approximately(
current_waypoint.r, max(1e-3, linear_tolerance), max(1e-3, angular_tolerance)) &&
current_waypoint.r.qdot == subjugator::Vector6d::Zero()) {
actionserver.setSucceeded();
}
}
double PID::update(double input, double x, double dx, const ros::Duration& dt)
{
if (!parameters_.enabled) return 0.0;
double dt_sec = dt.toSec();
// low-pass filter input
if (std::isnan(state_.input)) state_.input = input;
if (dt_sec + parameters_.time_constant > 0.0) {
state_.dinput = (input - state_.input) / (dt_sec + parameters_.time_constant);
state_.input = (dt_sec * input + parameters_.time_constant * state_.input) / (dt_sec + parameters_.time_constant);
}
return update(state_.input - x, dx, dt);
}
double Pid::updatePid(double error, ros::Duration dt)
{
// Get the gain parameters from the realtime buffer
Gains gains = *gains_buffer_.readFromRT();
double p_term, d_term, i_term;
p_error_ = error; //this is pError = pState-pTarget
if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
return 0.0;
// Calculate proportional contribution to command
p_term = gains.p_gain_ * p_error_;
// Calculate the integral of the position error
i_error_ += dt.toSec() * p_error_;
// Calculate integral contribution to command
i_term = gains.i_gain_ * i_error_;
// Limit i_term so that the limit is meaningful in the output
i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) );
// Calculate the derivative error
if (dt.toSec() > 0.0)
{
d_error_ = (p_error_ - p_error_last_) / dt.toSec();
p_error_last_ = p_error_;
}
// Calculate derivative contribution to command
d_term = gains.d_gain_ * d_error_;
// Compute the command
cmd_ = - p_term - i_term - d_term;
return cmd_;
}
int executeCB(ros::Duration dt)
{
std::cout << "**Walk -%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**Walk -%- execute_time: "
<< execute_time_.toSec() << std::endl;
execute_time_ += dt;
if(!init_)
{
set_feedback(RUNNING);
initialize();
}
double x = motion_proxy_ptr->getRobotPosition(true).at(0);
double y = motion_proxy_ptr->getRobotPosition(true).at(1);
double z = motion_proxy_ptr->getRobotPosition(true).at(2);
// Walk forward
double angular = 0.1*modulo2Pi(z_0-z);
//ROS_INFO("theta = %f",angular);
if(angular > 1) {angular = 1;}
if(angular < -1) {angular = -1;}
geometry_msgs::Twist cmd;
cmd.linear.x = 0.3; //0.5
cmd.angular.z = angular;
cmd_pub.publish(cmd);
if(sqrt((x-x_0)*(x-x_0) + (y-y_0)*(y-y_0)) > dist)
{
set_feedback(SUCCESS);
finalize();
return 1;
}
return 0;
}
string duration_string(const ros::Duration &d) {
char buf[40];
boost::posix_time::time_duration td = d.toBoost();
if (td.hours() > 0) {
snprintf(buf, sizeof(buf) / sizeof(buf[0]), "%dhr %d:%02ds (%ds)",
td.hours(), td.minutes(), td.seconds(), td.total_seconds());
} else if (td.minutes() > 0) {
snprintf(buf, sizeof(buf) / sizeof(buf[0]), "%d:%02ds (%ds)",
td.minutes(), td.seconds(), td.total_seconds());
} else {
snprintf(buf, sizeof(buf) / sizeof(buf[0]), "%.1fs", d.toSec());
}
return string(buf);
}
/**
* @brief TimeManipulator::str
* @param duration
* @return
*/
std::string TimeManipulator::str(const ros::Duration& duration)
{
double seconds = duration.toSec();
int hours = floor(seconds / 3600);
int minutes = floor(seconds / 60);
seconds = (seconds / 60 - minutes) * 60;
minutes -= hours * 60;
std::stringstream ss;
ss << hours << (minutes < 10 ? ":0" : ":") << minutes;
if (seconds > 0)
{
ss << (seconds < 10 ? ":0" : ":") << seconds;
}
return ss.str();
}
// TEST CASES
TEST_F(FourWheelSteeringControllerTest, testForward)
{
// wait for ROS
waitForController();
// zero everything before test
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = 0.0;
cmd_vel.angular.z = 0.0;
publish(cmd_vel);
ros::Duration(0.1).sleep();
// get initial odom
nav_msgs::Odometry old_odom = getLastOdom();
// send a velocity command of 0.1 m/s
cmd_vel.linear.x = 0.1;
publish(cmd_vel);
// wait for 10s
ros::Duration(10.0).sleep();
nav_msgs::Odometry new_odom = getLastOdom();
const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp;
const double expected_distance = cmd_vel.linear.x * actual_elapsed_time.toSec();
// check if the robot traveled 1 meter in XY plane, changes in z should be ~~0
const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
EXPECT_NEAR(sqrt(dx*dx + dy*dy), expected_distance, POSITION_TOLERANCE);
EXPECT_LT(fabs(dz), EPS);
// convert to rpy and test that way
double roll_old, pitch_old, yaw_old;
double roll_new, pitch_new, yaw_new;
tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
EXPECT_LT(fabs(roll_new - roll_old), EPS);
EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS);
EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
}
int main( int argc, char** argv )
{
const int startup_delay = 5;
ros::init(argc, argv, "rx60_hardware");
ros::NodeHandle nh_("robot_rx60b");
RX60Robot robot;
RX60_wrapper wrapper;
controller_manager::ControllerManager cm(&robot, nh_);
ros::Publisher state_publisher = nh_.advertise<sensor_msgs::JointState>("/robot_rx60b/controller_joint_states", 10);
ros::AsyncSpinner spinner(0);
spinner.start();
//ros::MultiThreadedSpinner spinner(4);
//spinner.spin();
ros::Time last = ros::Time::now();
ros::Rate r(10);
int alive_count = 0;
ros::Time startup_time = ros::Time::now();
while (ros::ok())
{
ros::Duration period = ros::Time::now() - last;
const sensor_msgs::JointState::Ptr robotState = wrapper.getJointState();
robot.read(robotState);
// Wait for valid jointstates
const ros::Duration time_since_start = ros::Time::now() - startup_time;
if (time_since_start.toSec() > startup_delay)
{
cm.update(ros::Time::now(), period);
const sensor_msgs::JointState::Ptr robotCmd = robot.write();
wrapper.setJointState(robotCmd);
}
else
ROS_INFO("Waiting %i seconds before controlling the robot (%f elapsed)", startup_delay, time_since_start.toSec());
last = ros::Time::now();
state_publisher.publish(robotState);
ros::spinOnce();
r.sleep();
}
}
void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state) {
if((ros::Time::now()-last_update_time_).toSec() < (MARKER_DUR.toSec()/2.0)) {
return;
}
last_update_time_ = ros::Time::now();
if(!send_markers_) return;
collision_models_->bodiesLock();
if(group_name_1_.empty() && group_name_2_.empty()) {
sendMarkersForGroup("");
}
if(!group_name_1_.empty()) {
sendMarkersForGroup(group_name_1_);
}
if(!group_name_2_.empty()) {
sendMarkersForGroup(group_name_2_);
}
collision_models_->bodiesUnlock();
}
double PID::update(double error, double dx, const ros::Duration& dt)
{
if (!parameters_.enabled) return 0.0;
if (std::isnan(error)) return 0.0;
double dt_sec = dt.toSec();
// integral error
state_.i += error * dt_sec;
if (parameters_.limit_i > 0.0)
{
if (state_.i > parameters_.limit_i) state_.i = parameters_.limit_i;
if (state_.i < -parameters_.limit_i) state_.i = -parameters_.limit_i;
}
// differential error
if (dt_sec > 0.0 && !std::isnan(state_.p) && !std::isnan(state_.dx)) {
state_.d = (error - state_.p) / dt_sec + state_.dx - dx;
} else {
state_.d = -dx;
}
state_.dx = dx;
// proportional error
state_.p = error;
// calculate output...
double output = parameters_.k_p * state_.p + parameters_.k_i * state_.i + parameters_.k_d * state_.d;
int antiwindup = 0;
if (parameters_.limit_output > 0.0)
{
if (output > parameters_.limit_output) {
output = parameters_.limit_output;
antiwindup = 1;
}
if (output < -parameters_.limit_output) {
output = -parameters_.limit_output;
antiwindup = -1;
}
}
if (antiwindup && (error * dt_sec * antiwindup > 0.0)) state_.i -= error * dt_sec;
checknan(output);
return output;
}
void JointVelocityController::update(const ros::Time& time, const ros::Duration& period)
{
double error = command_ - joint_.getVelocity();
double current_effort = joint_.getEffort();
double commanded_effort;
if(current_effort>effort_threshold)
{
commanded_effort=0.0;
}
else
{
// Set the PID error and compute the PID command with nonuniform time
// step size. The derivative error is computed from the change in the error
// and the timestep dt.
commanded_effort = pid_controller_.computeCommand(error, period);
}
joint_.setCommand(commanded_effort);
if(loop_count_ % 10 == 0)
{
if(controller_state_publisher_ && controller_state_publisher_->trylock())
{
controller_state_publisher_->msg_.header.stamp = time;
controller_state_publisher_->msg_.set_point = command_;
controller_state_publisher_->msg_.process_value = joint_.getVelocity();
controller_state_publisher_->msg_.error = error;
controller_state_publisher_->msg_.time_step = period.toSec();
controller_state_publisher_->msg_.command = commanded_effort;
double dummy;
getGains(controller_state_publisher_->msg_.p,
controller_state_publisher_->msg_.i,
controller_state_publisher_->msg_.d,
controller_state_publisher_->msg_.i_clamp,
dummy);
controller_state_publisher_->unlockAndPublish();
}
}
loop_count_++;
}
void NodeClass::sendEmergencyStopStates()
{
requestBoardStatus();
if(!relayboard_available) return;
bool EM_signal;
ros::Duration duration_since_EM_confirmed;
cob_relayboard::EmergencyStopState EM_msg;
// assign input (laser, button) specific EM state
EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();
// determine current EMStopState
EM_signal = (EM_msg.emergency_button_stop || EM_msg.scanner_stop);
switch (EM_stop_status_)
{
case ST_EM_FREE:
{
if (EM_signal == true)
{
ROS_INFO("Emergency stop was issued");
EM_stop_status_ = EM_msg.EMSTOP;
}
break;
}
case ST_EM_ACTIVE:
{
if (EM_signal == false)
{
ROS_INFO("Emergency stop was confirmed");
EM_stop_status_ = EM_msg.EMCONFIRMED;
time_of_EM_confirmed_ = ros::Time::now();
}
break;
}
case ST_EM_CONFIRMED:
{
if (EM_signal == true)
{
ROS_INFO("Emergency stop was issued");
EM_stop_status_ = EM_msg.EMSTOP;
}
else
{
duration_since_EM_confirmed = ros::Time::now() - time_of_EM_confirmed_;
if( duration_since_EM_confirmed.toSec() > duration_for_EM_free_.toSec() )
{
ROS_INFO("Emergency stop released");
EM_stop_status_ = EM_msg.EMFREE;
}
}
break;
}
};
EM_msg.emergency_state = EM_stop_status_;
//publish EM-Stop-Active-messages, when connection to relayboard got cut
if(relayboard_online == false) {
EM_msg.emergency_state = EM_msg.EMSTOP;
}
topicPub_isEmergencyStop.publish(EM_msg);
}
void NodeClass::sendEmergencyStopStates()
{
requestBoardStatus();
if(!relayboard_available) return;
sendBatteryVoltage();
bool EM_signal;
ros::Duration duration_since_EM_confirmed;
cob_relayboard::EmergencyStopState EM_msg;
pr2_msgs::PowerBoardState pbs;
pbs.header.stamp = ros::Time::now();
// assign input (laser, button) specific EM state TODO: Laser and Scanner stop can't be read independently (e.g. if button is stop --> no informtion about scanner, if scanner ist stop --> no informtion about button stop)
EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();
// determine current EMStopState
EM_signal = (EM_msg.emergency_button_stop || EM_msg.scanner_stop);
switch (EM_stop_status_)
{
case ST_EM_FREE:
{
if (EM_signal == true)
{
ROS_INFO("Emergency stop was issued");
EM_stop_status_ = EM_msg.EMSTOP;
}
break;
}
case ST_EM_ACTIVE:
{
if (EM_signal == false)
{
ROS_INFO("Emergency stop was confirmed");
EM_stop_status_ = EM_msg.EMCONFIRMED;
time_of_EM_confirmed_ = ros::Time::now();
}
break;
}
case ST_EM_CONFIRMED:
{
if (EM_signal == true)
{
ROS_INFO("Emergency stop was issued");
EM_stop_status_ = EM_msg.EMSTOP;
}
else
{
duration_since_EM_confirmed = ros::Time::now() - time_of_EM_confirmed_;
if( duration_since_EM_confirmed.toSec() > duration_for_EM_free_.toSec() )
{
ROS_INFO("Emergency stop released");
EM_stop_status_ = EM_msg.EMFREE;
}
}
break;
}
};
EM_msg.emergency_state = EM_stop_status_;
// pr2 power_board_state
if(EM_msg.emergency_button_stop)
pbs.run_stop = false;
else
pbs.run_stop = true;
//for cob the wireless stop field is misused as laser stop field
if(EM_msg.scanner_stop)
pbs.wireless_stop = false;
else
pbs.wireless_stop = true;
//publish EM-Stop-Active-messages, when connection to relayboard got cut
if(relayboard_online == false) {
EM_msg.emergency_state = EM_msg.EMSTOP;
}
topicPub_isEmergencyStop.publish(EM_msg);
topicPub_PowerBoardState.publish(pbs);
}
void targetsCallback(const mtt::TargetList& list)
{
//will print information that should be stored in a file
//file format: id, good/bad tag, time, pos x, pos y, vel, theta
// position_diff, heading_diff, angle_to_robot, velocity_diff
static ros::Time start_time = ros::Time::now();
time_elapsed = ros::Time::now() - start_time;
/// /// ROBOT PART //////
//use transformations to extract robot features
try{
p_listener->lookupTransform("/map", "/base_link", ros::Time(0), transform);
p_listener->lookupTwist("/map", "/base_link", ros::Time(0), ros::Duration(0.5), twist);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
robot_x = transform.getOrigin().x();
robot_posex_buffer.push_back(robot_x);
robot_y = transform.getOrigin().y();
robot_theta = tf::getYaw(transform.getRotation());
robot_vel = sqrt(pow(twist.linear.x, 2) + pow(twist.linear.y, 2));
//robot output line
/// uncomment the following for training!
printf("%d,%d,%.10f,%.10f,%.10f,%.10f,%.10f,0,0,0,0\n",
-1, leader_tag, time_elapsed.toSec(),
robot_x, robot_y, robot_vel, robot_theta);
//testing new features extraction
// accumulator_set<double, stats<tag::variance> > acc;
// for_each(robot_posex_buffer.begin(), robot_posex_buffer.end(), boost::bind<void>(boost::ref(acc), _1));
// printf("%f,%f,%f\n", robot_x, mean(acc), sqrt(variance(acc)));
/// /// TARGETS PART //////
//sweeps target list and extract features
for(uint i = 0; i < list.Targets.size(); i++){
target_id = list.Targets[i].id;
target_x = list.Targets[i].pose.position.x;
target_y = list.Targets[i].pose.position.y;
target_theta = tf::getYaw(list.Targets[i].pose.orientation);
target_vel = sqrt(pow(list.Targets[i].velocity.linear.x,2)+
pow(list.Targets[i].velocity.linear.y,2));
position_diff = sqrt(pow(robot_x - target_x,2)+
pow(robot_y - target_y,2));
heading_diff = robot_theta - target_theta;
angle_to_robot = -robot_theta + atan2(target_y - robot_y,
target_x - robot_x );
velocity_diff = robot_vel - target_vel;
//target output (to be used in adaboost training)
// % output file format:
// % 1: id
// % 2: good/bad tag
// % 3: time
// % 4: pos x
// % 5: pos y
// % 6: vel
// % 7: theta
// % 8: pos diff
// % 9: head diff
// %10: angle 2 robot
// %11: velocity diff
/// uncomment the following to generate training file!
printf("%d,%d,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f\n",
target_id, leader_tag, time_elapsed.toSec(),
target_x, target_y, target_vel, target_theta,
position_diff, heading_diff, angle_to_robot, velocity_diff);
// if(position_diff < 6.0 && target_vel > 0.5){
// //if inside boundaries (in meters)
// //store features in a covariance struct to send to matlab
// nfeatures.pose.position.x = target_x;
// nfeatures.pose.position.y = target_y;
// nfeatures.pose.position.z = target_id;
//
// nfeatures.covariance[0] = target_vel;
// nfeatures.covariance[1] = velocity_diff;
// nfeatures.covariance[2] = heading_diff;
// nfeatures.covariance[3] = angle_to_robot;
// nfeatures.covariance[4] = position_diff;
//
// matlab_list.push_back(nfeatures);
// // counter++;
// }
}
// printf("targets within range: %d\n",counter);
// counter = 0;
//check if enough time has passed
//and send batch of msgs to matlab
// duration_btw_msg = ros::Time::now() - time_last_msg;
//
// if(duration_btw_msg.toSec() > 0.01){
// while(!matlab_list.empty()){
// nfeatures_pub.publish(matlab_list.front());
// matlab_list.pop_front();
// usleep(0.01e6); //has to sleep, otherwise matlab do not get the msg
// }
// time_last_msg = ros::Time::now();
// }
}
int executeCB(ros::Duration dt)
{
std::cout << "**GoToWaypoint -%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**GoToWaypoint -%- execute_time: "
<< execute_time_.toSec() << std::endl;
execute_time_ += dt;
std::cout << "**Counter value:" << counter << std::endl;
if (counter > 1)
std::cout << "********************************************************************************" << std::endl;
if (!init_)
{
initialize();
init_ = true;
}
if ( (ros::Time::now() - time_at_pos_).toSec() < 0.2 )
{
if( message_.type == "goto")
{
// Goal position of ball relative to ROBOT_FRAME
float goal_x = 0.00;
float goal_y = 0.00;
float error_x = message_.x - goal_x;
float error_y = message_.y - goal_y;
if (fabs(error_x) < 0.12 && fabs(error_y) < 0.12)
{
std::cout << "Closeness count " << closeness_count << std::endl;
closeness_count++;
//If the NAO has been close for enough iterations, we consider to goal reached
if (closeness_count > 0)
{
motion_proxy_ptr->stopMove();
set_feedback(SUCCESS);
// std::cout << "sleeeping for 2 second before destroying thread" << std::endl;
// sleep(2.0);
finalize();
return 1;
}
}
else
{
closeness_count = 0;
}
//Limit the "error" in order to limit the walk speed
error_x = error_x > 0.6 ? 0.6 : error_x;
error_x = error_x < -0.6 ? -0.6 : error_x;
error_y = error_y > 0.6 ? 0.6 : error_y;
error_y = error_y < -0.6 ? -0.6 : error_y;
// float speed_x = error_x * 1.0/(2+5*closeness_count);
// float speed_y = error_y * 1.0/(2+5*closeness_count);
// float frequency = 0.1/(5*closeness_count+(1.0/(fabs(error_x)+fabs(error_y)))); //Frequency of foot steps
// motion_proxy_ptr->setWalkTargetVelocity(speed_x, speed_y, 0.0, frequency);
// ALMotionProxy::setWalkTargetVelocity(const float& x, const float& y, const float& theta, const float& frequency)
AL::ALValue walk_config;
//walk_config.arrayPush(AL::ALValue::array("MaxStepFrequency", frequency));
//Lower value of step height gives smoother walking
// std::cout << "y " << message_.y << std::endl;
if (fabs(message_.y) < 0.10)
{
// walk_config.arrayPush(AL::ALValue::array("StepHeight", 0.01));
// motion_proxy_ptr->post.moveTo(message_.x, 0.0, 0.0, walk_config);
motion_proxy_ptr->post.moveTo(message_.x, 0.0, 0.0);
sleep(2.0);
//motion_proxy_ptr->post.stopMove();
}
else
{
// walk_config.arrayPush(AL::ALValue::array("StepHeight", 0.005));
// motion_proxy_ptr->post.moveTo(0.0, 0.0, message_.y/fabs(message_.y)*0.1, walk_config);
motion_proxy_ptr->post.moveTo(0.0, 0.0, message_.y/fabs(message_.y)*0.1);
//sleep(3.0);
//motion_proxy_ptr->post.stopMove();
}
}
}
set_feedback(RUNNING);
return 0;
}
void DiffDriveController::update(const ros::Time& time, const ros::Duration& period)
{
// COMPUTE AND PUBLISH ODOMETRY
if (open_loop_)
{
odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
}
else
{
double left_pos = 0.0;
double right_pos = 0.0;
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
const double lp = left_wheel_joints_[i].getPosition();
const double rp = right_wheel_joints_[i].getPosition();
if (std::isnan(lp) || std::isnan(rp))
return;
left_pos += lp;
right_pos += rp;
}
left_pos /= wheel_joints_size_;
right_pos /= wheel_joints_size_;
// Estimate linear and angular velocity using joint information
odometry_.update(left_pos, right_pos, time);
}
// Publish odometry message
if (last_state_publish_time_ + publish_period_ < time)
{
last_state_publish_time_ += publish_period_;
// Compute and store orientation info
const geometry_msgs::Quaternion orientation(
tf::createQuaternionMsgFromYaw(odometry_.getHeading()));
// Populate odom message and publish
if (odom_pub_->trylock())
{
odom_pub_->msg_.header.stamp = time;
odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
odom_pub_->msg_.pose.pose.orientation = orientation;
odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear();
odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
odom_pub_->unlockAndPublish();
}
// Publish tf /odom frame
if (enable_odom_tf_ && tf_odom_pub_->trylock())
{
geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
odom_frame.header.stamp = time;
odom_frame.transform.translation.x = odometry_.getX();
odom_frame.transform.translation.y = odometry_.getY();
odom_frame.transform.rotation = orientation;
tf_odom_pub_->unlockAndPublish();
}
}
// MOVE ROBOT
// Retreive current velocity command and time step:
Commands curr_cmd = *(command_.readFromRT());
const double dt = (time - curr_cmd.stamp).toSec();
// Brake if cmd_vel has timeout:
if (dt > cmd_vel_timeout_)
{
curr_cmd.lin = 0.0;
curr_cmd.ang = 0.0;
}
// Limit velocities and accelerations:
const double cmd_dt(period.toSec());
limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
last1_cmd_ = last0_cmd_;
last0_cmd_ = curr_cmd;
// Apply multipliers:
const double ws = wheel_separation_multiplier_ * wheel_separation_;
const double wr = wheel_radius_multiplier_ * wheel_radius_;
// Compute wheels velocities:
const double vel_left = (curr_cmd.lin - curr_cmd.ang * ws / 2.0)/wr;
const double vel_right = (curr_cmd.lin + curr_cmd.ang * ws / 2.0)/wr;
// Set wheels velocities:
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
left_wheel_joints_[i].setCommand(vel_left);
right_wheel_joints_[i].setCommand(vel_right);
}
}
void read(ros::Time time, ros::Duration period)
{
for(int j=0; j < n_joints_; ++j)
{
joint_position_prev_[j] = joint_position_[j];
joint_position_[j] += angles::shortest_angular_distance(joint_position_[j],
sim_joints_[j]->GetAngle(0).Radian());
joint_position_kdl_(j) = joint_position_[j];
// derivate velocity as in the real hardware instead of reading it from simulation
joint_velocity_[j] = filters::exponentialSmoothing((joint_position_[j] - joint_position_prev_[j])/period.toSec(), joint_velocity_[j], 0.2);
joint_effort_[j] = sim_joints_[j]->GetForce((int)(0));
joint_stiffness_[j] = joint_stiffness_command_[j];
}
}
void DynamicSlidingModeControllerTaskSpace::update(const ros::Time& time, const ros::Duration& period)
{
// get joint positions
for(int i=0; i < joint_handles_.size(); i++)
{
joint_msr_states_.q(i) = joint_handles_[i].getPosition();
joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
}
if (cmd_flag_)
{
// computing forward kinematics
fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);
// computing Jacobian J(q)
jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);
// computing Jacobian pseudo-inversion
pseudo_inverse(J_.data,J_pinv_);
// computing end-effector position/orientation error w.r.t. desired frame
x_err_ = diff(x_,x_des_);
/* Trying quaternions, it seems to work better
// end-effector position/orientation error
x_err_.vel = (x_des_.p - x_.p);
// getting quaternion from rotation matrix
x_.M.GetQuaternion(quat_curr_.v(0),quat_curr_.v(1),quat_curr_.v(2),quat_curr_.a);
x_des_.M.GetQuaternion(quat_des_.v(0),quat_des_.v(1),quat_des_.v(2),quat_des_.a);
skew_symmetric(quat_des_.v,skew_);
for (int i = 0; i < skew_.rows(); i++)
{
v_temp_(i) = 0.0;
for (int k = 0; k < skew_.cols(); k++)
v_temp_(i) += skew_(i,k)*(quat_curr_.v(k));
}
x_err_.rot = (quat_curr_.a*quat_des_.v - quat_des_.a*quat_curr_.v) - v_temp_;
*/
// clearing error msg before publishing
msg_err_.data.clear();
for(int i = 0; i < e_ref_.size(); i++)
{
e_ref_(i) = x_err_(i);
msg_err_.data.push_back(e_ref_(i));
}
joint_des_states_.qdot.data = J_pinv_*e_ref_;
joint_des_states_.q.data = joint_msr_states_.q.data + period.toSec()*joint_des_states_.qdot.data;
// computing S
S_.data = (joint_msr_states_.qdot.data - joint_des_states_.qdot.data) + alpha_.data.cwiseProduct(joint_msr_states_.q.data - joint_des_states_.q.data);
//for (int i = 0; i < joint_handles_.size(); i++)
//S_(i) = (joint_msr_states_.qdot(i) - joint_des_states_.qdot(i)) + alpha_(i)*tanh(lambda_(i)*(joint_msr_states_.q(i) - joint_des_states_.q(i)));
// saving S0 on the first step
if (step_ == 0)
S0_ = S_;
// computing Sd
for (int i = 0; i < joint_handles_.size(); i++)
Sd_(i) = S0_(i)*exp(-k_(i)*(step_*period.toSec()));
Sq_.data = S_.data + Sd_.data;//- Sd_.data;
// computing sigma_dot as sgn(Sq)
for (int i = 0; i < joint_handles_.size(); i++)
sigma_dot_(i) = -(Sq_(i) < 0) + (Sq_(i) > 0);
// integrating sigma_dot
sigma_.data += period.toSec()*sigma_dot_.data;
//for (int i = 0; i < joint_handles_.size(); i++)
//sigma_(i) += period.toSec()*pow(Sq_(i),0.5);
// computing Sr
Sr_.data = Sq_.data + gamma_.data.cwiseProduct(sigma_.data);
// computing tau
tau_.data = -Kd_.data.cwiseProduct(Sr_.data);
step_++;
if (Equal(x_,x_des_,0.05))
{
ROS_INFO("On target");
cmd_flag_ = 0;
return;
}
}
// set controls for joints
for (int i = 0; i < joint_handles_.size(); i++)
{
if (!cmd_flag_)
tau_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period);//Kd_(i)*(alpha_(i)*(joint_des_states_.q(i) - joint_msr_states_.q(i)) + (joint_des_states_.qdot(i) - joint_msr_states_.qdot(i))) ;
joint_handles_[i].setCommand(tau_(i));
}
// publishing markers for visualization in rviz
pub_marker_.publish(msg_marker_);
msg_id_++;
// publishing error for all tasks as an array of ntasks*6
pub_error_.publish(msg_err_);
// publishing actual and desired trajectory for each task (links) as an array of ntasks*3
pub_pose_.publish(msg_pose_);
pub_traj_.publish(msg_traj_);
ros::spinOnce();
}
void TaskInverseKinematics::update(const ros::Time& time, const ros::Duration& period)
{
// get joint positions
for(int i=0; i < joint_handles_.size(); i++)
{
joint_msr_states_.q(i) = joint_handles_[i].getPosition();
joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
}
if (cmd_flag_)
{
// computing Jacobian
jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);
// computing J_pinv_
pseudo_inverse(J_.data,J_pinv_);
// computing forward kinematics
fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);
// end-effector position/orientation error
x_err_.vel = x_des_.p - x_.p;
// getting quaternion from rotation matrix
x_.M.GetQuaternion(quat_curr_.v(0),quat_curr_.v(1),quat_curr_.v(2),quat_curr_.a);
x_des_.M.GetQuaternion(quat_des_.v(0),quat_des_.v(1),quat_des_.v(2),quat_des_.a);
skew_symmetric(quat_des_.v,skew_);
for (int i = 0; i < skew_.rows(); i++)
{
v_temp_(i) = 0.0;
for (int k = 0; k < skew_.cols(); k++)
v_temp_(i) += skew_(i,k)*(quat_curr_.v(k));
}
x_err_.rot = quat_curr_.a*quat_des_.v - quat_des_.a*quat_curr_.v - v_temp_;
// computing q_dot
for (int i = 0; i < J_pinv_.rows(); i++)
{
joint_des_states_.qdot(i) = 0.0;
for (int k = 0; k < J_pinv_.cols(); k++)
joint_des_states_.qdot(i) += J_pinv_(i,k)*x_err_(k); //removed scaling factor of .7
}
// integrating q_dot -> getting q (Euler method)
for (int i = 0; i < joint_handles_.size(); i++)
joint_des_states_.q(i) += period.toSec()*joint_des_states_.qdot(i);
// joint limits saturation
for (int i =0; i < joint_handles_.size(); i++)
{
if (joint_des_states_.q(i) < joint_limits_.min(i))
joint_des_states_.q(i) = joint_limits_.min(i);
if (joint_des_states_.q(i) > joint_limits_.max(i))
joint_des_states_.q(i) = joint_limits_.max(i);
}
if (Equal(x_,x_des_,0.005))
{
ROS_INFO("On target");
cmd_flag_ = 0;
}
}
// set controls for joints
for (int i = 0; i < joint_handles_.size(); i++)
{
tau_cmd_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period);
joint_handles_[i].setCommand(tau_cmd_(i));
}
}
void JointPositionController::update(const ros::Time& time, const ros::Duration& period)
{
command_struct_ = *(command_.readFromRT());
double command_position = command_struct_.position_;
double command_velocity = command_struct_.velocity_;
bool has_velocity_ = command_struct_.has_velocity_;
double error, vel_error;
double commanded_effort;
double current_position = joint_.getPosition();
// Make sure joint is within limits if applicable
enforceJointLimits(command_position);
// Compute position error
if (joint_urdf_->type == urdf::Joint::REVOLUTE)
{
angles::shortest_angular_distance_with_limits(
current_position,
command_position,
joint_urdf_->limits->lower,
joint_urdf_->limits->upper,
error);
}
else if (joint_urdf_->type == urdf::Joint::CONTINUOUS)
{
error = angles::shortest_angular_distance(current_position, command_position);
}
else //prismatic
{
error = command_position - current_position;
}
// Decide which of the two PID computeCommand() methods to call
if (has_velocity_)
{
// Compute velocity error if a non-zero velocity command was given
vel_error = command_velocity - joint_.getVelocity();
// Set the PID error and compute the PID command with nonuniform
// time step size. This also allows the user to pass in a precomputed derivative error.
commanded_effort = pid_controller_.computeCommand(error, vel_error, period);
}
else
{
// Set the PID error and compute the PID command with nonuniform
// time step size.
commanded_effort = pid_controller_.computeCommand(error, period);
}
joint_.setCommand(commanded_effort);
// publish state
if (loop_count_ % 10 == 0)
{
if(controller_state_publisher_ && controller_state_publisher_->trylock())
{
controller_state_publisher_->msg_.header.stamp = time;
controller_state_publisher_->msg_.set_point = command_position;
controller_state_publisher_->msg_.process_value = current_position;
controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity();
controller_state_publisher_->msg_.error = error;
controller_state_publisher_->msg_.time_step = period.toSec();
controller_state_publisher_->msg_.command = commanded_effort;
double dummy;
bool antiwindup;
getGains(controller_state_publisher_->msg_.p,
controller_state_publisher_->msg_.i,
controller_state_publisher_->msg_.d,
controller_state_publisher_->msg_.i_clamp,
dummy,
antiwindup);
controller_state_publisher_->msg_.antiwindup = static_cast<char>(antiwindup);
controller_state_publisher_->unlockAndPublish();
}
}
loop_count_++;
}
void
AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
last_laser_received_ts_ = ros::Time::now();
if( map_ == NULL ) {
return;
}
boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
int laser_index = -1;
// Do we have the base->base_laser Tx yet?
if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
{
ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan->header.frame_id.c_str());
lasers_.push_back(new AMCLLaser(*laser_));
lasers_update_.push_back(true);
laser_index = frame_to_laser_.size();
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
tf::Vector3(0,0,0)),
ros::Time(), laser_scan->header.frame_id);
tf::Stamped<tf::Pose> laser_pose;
try
{
this->tf_->transformPose(base_frame_id_, ident, laser_pose);
}
catch(tf::TransformException& e)
{
ROS_ERROR("Couldn't transform from %s to %s, "
"even though the message notifier is in use",
laser_scan->header.frame_id.c_str(),
base_frame_id_.c_str());
return;
}
pf_vector_t laser_pose_v;
laser_pose_v.v[0] = laser_pose.getOrigin().x();
laser_pose_v.v[1] = laser_pose.getOrigin().y();
// laser mounting angle gets computed later -> set to 0 here!
laser_pose_v.v[2] = 0;
lasers_[laser_index]->SetLaserPose(laser_pose_v);
ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
laser_pose_v.v[0],
laser_pose_v.v[1],
laser_pose_v.v[2]);
frame_to_laser_[laser_scan->header.frame_id] = laser_index;
} else {
// we have the laser pose, retrieve laser index
laser_index = frame_to_laser_[laser_scan->header.frame_id];
}
// Where was the robot when this scan was taken?
tf::Stamped<tf::Pose> odom_pose;
pf_vector_t pose;
if(!getOdomPose(odom_pose, pose.v[0], pose.v[1], pose.v[2],
laser_scan->header.stamp, base_frame_id_))
{
ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
return;
}
pf_vector_t delta = pf_vector_zero();
if(pf_init_)
{
// Compute change in pose
//delta = pf_vector_coord_sub(pose, pf_odom_pose_);
delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
// See if we should update the filter
bool update = fabs(delta.v[0]) > d_thresh_ ||
fabs(delta.v[1]) > d_thresh_ ||
fabs(delta.v[2]) > a_thresh_;
update = update || m_force_update;
m_force_update=false;
// Set the laser update flags
if(update)
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true;
}
bool force_publication = false;
if(!pf_init_)
{
// Pose at last filter update
pf_odom_pose_ = pose;
// Filter is now initialized
pf_init_ = true;
// Should update sensor data
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true;
force_publication = true;
resample_count_ = 0;
}
// If the robot has moved, update the filter
else if(pf_init_ && lasers_update_[laser_index])
{
//printf("pose\n");
//pf_vector_fprintf(pose, stdout, "%.3f");
AMCLOdomData odata;
odata.pose = pose;
// HACK
// Modify the delta in the action data so the filter gets
// updated correctly
odata.delta = delta;
// Use the action data to update the filter
odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
// Pose at last filter update
//this->pf_odom_pose = pose;
}
bool resampled = false;
// If the robot has moved, update the filter
if(lasers_update_[laser_index])
{
AMCLLaserData ldata;
ldata.sensor = lasers_[laser_index];
ldata.range_count = laser_scan->ranges.size();
// To account for lasers that are mounted upside-down, we determine the
// min, max, and increment angles of the laser in the base frame.
//
// Construct min and max angles of laser, in the base_link frame.
tf::Quaternion q;
q.setRPY(0.0, 0.0, laser_scan->angle_min);
tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
laser_scan->header.frame_id);
q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
laser_scan->header.frame_id);
try
{
tf_->transformQuaternion(base_frame_id_, min_q, min_q);
tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
return;
}
double angle_min = tf::getYaw(min_q);
double angle_increment = tf::getYaw(inc_q) - angle_min;
// wrapping angle to [-pi .. pi]
angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;
ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);
// Apply range min/max thresholds, if the user supplied them
if(laser_max_range_ > 0.0)
ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
else
ldata.range_max = laser_scan->range_max;
double range_min;
if(laser_min_range_ > 0.0)
range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
else
range_min = laser_scan->range_min;
// The AMCLLaserData destructor will free this memory
ldata.ranges = new double[ldata.range_count][2];
ROS_ASSERT(ldata.ranges);
for(int i=0; i<ldata.range_count; i++)
{
// amcl doesn't (yet) have a concept of min range. So we'll map short
// readings to max range.
if(laser_scan->ranges[i] <= range_min)
ldata.ranges[i][0] = ldata.range_max;
else
ldata.ranges[i][0] = laser_scan->ranges[i];
// Compute bearing
ldata.ranges[i][1] = angle_min +
(i * angle_increment);
}
lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
lasers_update_[laser_index] = false;
pf_odom_pose_ = pose;
// Resample the particles
if(!(++resample_count_ % resample_interval_))
{
pf_update_resample(pf_);
resampled = true;
}
pf_sample_set_t* set = pf_->sets + pf_->current_set;
ROS_DEBUG("Num samples: %d\n", set->sample_count);
// Publish the resulting cloud
// TODO: set maximum rate for publishing
if (!m_force_update) {
geometry_msgs::PoseArray cloud_msg;
cloud_msg.header.stamp = ros::Time::now();
cloud_msg.header.frame_id = global_frame_id_;
cloud_msg.poses.resize(set->sample_count);
for(int i=0; i<set->sample_count; i++)
{
tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]),
tf::Vector3(set->samples[i].pose.v[0],
set->samples[i].pose.v[1], 0)),
cloud_msg.poses[i]);
}
particlecloud_pub_.publish(cloud_msg);
}
}
if(resampled || force_publication)
{
// Read out the current hypotheses
double max_weight = 0.0;
int max_weight_hyp = -1;
std::vector<amcl_hyp_t> hyps;
hyps.resize(pf_->sets[pf_->current_set].cluster_count);
for(int hyp_count = 0;
hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
{
double weight;
pf_vector_t pose_mean;
pf_matrix_t pose_cov;
if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
{
ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
break;
}
hyps[hyp_count].weight = weight;
hyps[hyp_count].pf_pose_mean = pose_mean;
hyps[hyp_count].pf_pose_cov = pose_cov;
if(hyps[hyp_count].weight > max_weight)
{
max_weight = hyps[hyp_count].weight;
max_weight_hyp = hyp_count;
}
}
if(max_weight > 0.0)
{
ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);
/*
puts("");
pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
puts("");
*/
geometry_msgs::PoseWithCovarianceStamped p;
// Fill in the header
p.header.frame_id = global_frame_id_;
p.header.stamp = laser_scan->header.stamp;
// Copy in the pose
p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
p.pose.pose.orientation);
// Copy in the covariance, converting from 3-D to 6-D
pf_sample_set_t* set = pf_->sets + pf_->current_set;
for(int i=0; i<2; i++)
{
for(int j=0; j<2; j++)
{
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
p.pose.covariance[6*i+j] = set->cov.m[i][j];
}
}
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
p.pose.covariance[6*5+5] = set->cov.m[2][2];
/*
printf("cov:\n");
for(int i=0; i<6; i++)
{
for(int j=0; j<6; j++)
printf("%6.3f ", p.covariance[6*i+j]);
puts("");
}
*/
pose_pub_.publish(p);
last_published_pose = p;
ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);
// subtracting base to odom from map to base and send map to odom instead
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));
tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
laser_scan->header.stamp,
base_frame_id_);
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
}
catch(tf::TransformException)
{
ROS_DEBUG("Failed to subtract base to odom transform");
return;
}
latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
tf::Point(odom_to_map.getOrigin()));
latest_tf_valid_ = true;
if (tf_broadcast_ == true)
{
// We want to send a transform that is good up until a
// tolerance time so that odom can be used
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}
}
else
{
ROS_ERROR("No pose!");
}
}
else if(latest_tf_valid_)
{
if (tf_broadcast_ == true)
{
// Nothing changed, so we'll just republish the last transform, to keep
// everybody happy.
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
}
// Is it time to save our last pose to the param server
ros::Time now = ros::Time::now();
if((save_pose_period.toSec() > 0.0) &&
(now - save_pose_last_time) >= save_pose_period)
{
// We need to apply the last transform to the latest odom pose to get
// the latest map pose to store. We'll take the covariance from
// last_published_pose.
tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
double yaw,pitch,roll;
map_pose.getBasis().getEulerYPR(yaw, pitch, roll);
private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x());
private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y());
private_nh_.setParam("initial_pose_a", yaw);
private_nh_.setParam("initial_cov_xx",
last_published_pose.pose.covariance[6*0+0]);
private_nh_.setParam("initial_cov_yy",
last_published_pose.pose.covariance[6*1+1]);
private_nh_.setParam("initial_cov_aa",
last_published_pose.pose.covariance[6*5+5]);
save_pose_last_time = now;
}
}
}
bool IIWA_HW::read(ros::Duration period)
{
ros::Duration delta = ros::Time::now() - timer_;
static bool was_connected = false;
if (iiwa_ros_conn_.getRobotIsConnected()) {
iiwa_ros_conn_.getJointPosition(joint_position_);
iiwa_ros_conn_.getJointTorque(joint_torque_);
device_->joint_position_prev = device_->joint_position;
iiwaMsgsJointToVector(joint_position_.position, device_->joint_position);
iiwaMsgsJointToVector(joint_torque_.torque, device_->joint_effort);
// if there is no controller active the robot goes to zero position
if (!was_connected) {
for (int j = 0; j < IIWA_JOINTS; j++)
device_->joint_position_command[j] = device_->joint_position[j];
was_connected = true;
}
for (int j = 0; j < IIWA_JOINTS; j++)
device_->joint_velocity[j] = filters::exponentialSmoothing((device_->joint_position[j]-device_->joint_position_prev[j])/period.toSec(),
device_->joint_velocity[j], 0.2);
return 1;
} else if (delta.toSec() >= 10) {
ROS_INFO("No LBR IIWA is connected. Waiting for the robot to connect before reading ...");
timer_ = ros::Time::now();
}
return 0;
}
// read 'measurement' joint values
void PanTiltHW::read(ros::Time time, ros::Duration period)
{
client_->Receive();
for(int i=0; i<n_joints_;i++)
{
joint_position_[i] = client_->drives[i].state.position;
joint_position_prev_[i] = joint_position_[i];
joint_velocity_[i] = filters::exponentialSmoothing((joint_position_[i] - joint_position_prev_[i])/period.toSec(), joint_velocity_[i], 0.2);
}
}
void timer_callback(const ros::TimerEvent &)
{
mil_msgs::MoveToResult actionresult;
// Handle disabled, killed, or no odom before attempting to produce trajectory
std::string err = "";
if (disabled)
err = "c3 disabled";
else if (kill_listener.isRaised())
err = "killed";
else if (!c3trajectory)
err = "no odom";
if (!err.empty())
{
if (c3trajectory)
c3trajectory.reset(); // On revive/enable, wait for odom before station keeping
// Cancel all goals while killed/disabled/no odom
if (actionserver.isNewGoalAvailable())
actionserver.acceptNewGoal();
if (actionserver.isActive())
{
actionresult.error = err;
actionresult.success = false;
actionserver.setAborted(actionresult);
}
return;
}
ros::Time now = ros::Time::now();
auto old_waypoint = current_waypoint;
if (actionserver.isNewGoalAvailable())
{
boost::shared_ptr<const mil_msgs::MoveToGoal> goal = actionserver.acceptNewGoal();
current_waypoint =
subjugator::C3Trajectory::Waypoint(Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist),
goal->speed, !goal->uncoordinated, !goal->blind);
current_waypoint_t = now;
this->linear_tolerance = goal->linear_tolerance;
this->angular_tolerance = goal->angular_tolerance;
waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::GREEN);
// Check if waypoint is valid
std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult = waypoint_validity_.is_waypoint_valid(
Pose_from_Waypoint(current_waypoint), current_waypoint.do_waypoint_validation);
actionresult.error = WAYPOINT_ERROR_TO_STRING.at(checkWPResult.second);
actionresult.success = checkWPResult.first;
if (checkWPResult.first == false && waypoint_check_) // got a point that we should not move to
{
waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::RED);
if (checkWPResult.second ==
WAYPOINT_ERROR_TYPE::UNKNOWN) // if unknown, check if there's a huge displacement with the new waypoint
{
auto a_point = Pose_from_Waypoint(current_waypoint);
auto b_point = Pose_from_Waypoint(old_waypoint);
// If moved more than .5m, then don't allow
if (abs(a_point.position.x - b_point.position.x) > .5 || abs(a_point.position.y - b_point.position.y) > .5)
{
ROS_ERROR("can't move there! - need to rotate");
current_waypoint = old_waypoint;
}
}
// if point is occupied, reject move
if (checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED)
{
ROS_ERROR("can't move there! - waypoint is occupied");
current_waypoint = old_waypoint;
}
// if point is above water, reject move
if (checkWPResult.second == WAYPOINT_ERROR_TYPE::ABOVE_WATER)
{
ROS_ERROR("can't move there! - waypoint is above water");
current_waypoint = old_waypoint;
}
if (checkWPResult.second == WAYPOINT_ERROR_TYPE::NO_OGRID)
{
ROS_ERROR("WaypointValidity - Did not recieve any ogrid");
}
}
}
if (actionserver.isPreemptRequested())
{
current_waypoint = c3trajectory->getCurrentPoint();
current_waypoint.do_waypoint_validation = false;
current_waypoint.r.qdot = subjugator::Vector6d::Zero(); // zero velocities
current_waypoint_t = now;
// don't try to make output c3 continuous when cancelled - instead stop as quickly as possible
c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
c3trajectory_t = now;
}
// Remember the previous trajectory
auto old_trajectory = c3trajectory->getCurrentPoint();
while (c3trajectory_t + traj_dt < now)
{
c3trajectory->update(traj_dt.toSec(), current_waypoint, (c3trajectory_t - current_waypoint_t).toSec());
c3trajectory_t += traj_dt;
}
// Check if we will hit something while in trajectory the new trajectory
geometry_msgs::Pose traj_point; // Convert messages to correct type
auto p = c3trajectory->getCurrentPoint();
traj_point.position = vec2xyz<Point>(p.q.head(3));
quaternionTFToMsg(tf::createQuaternionFromRPY(p.q[3], p.q[4], p.q[5]), traj_point.orientation);
std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult =
waypoint_validity_.is_waypoint_valid(Pose_from_Waypoint(p), c3trajectory->do_waypoint_validation);
if (checkWPResult.first == false && checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED && waypoint_check_)
{ // New trajectory will hit an occupied goal, so reject
ROS_ERROR("can't move there! - bad trajectory");
current_waypoint = old_trajectory;
current_waypoint.do_waypoint_validation = false;
current_waypoint.r.qdot = subjugator::Vector6d::Zero(); // zero velocities
current_waypoint_t = now;
c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
c3trajectory_t = now;
actionresult.success = false;
actionresult.error = WAYPOINT_ERROR_TO_STRING.at(WAYPOINT_ERROR_TYPE::OCCUPIED_TRAJECTORY);
}
PoseTwistStamped msg;
msg.header.stamp = c3trajectory_t;
msg.header.frame_id = fixed_frame;
msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint());
trajectory_pub.publish(msg);
waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(c3trajectory->getCurrentPoint()), 200);
PoseStamped msgVis;
msgVis.header = msg.header;
msgVis.pose = msg.posetwist.pose;
trajectory_vis_pub.publish(msgVis);
PoseStamped posemsg;
posemsg.header.stamp = c3trajectory_t;
posemsg.header.frame_id = fixed_frame;
posemsg.pose = Pose_from_Waypoint(current_waypoint);
waypoint_pose_pub.publish(posemsg);
if (actionserver.isActive() &&
c3trajectory->getCurrentPoint().is_approximately(current_waypoint.r, max(1e-3, linear_tolerance),
max(1e-3, angular_tolerance)) &&
current_waypoint.r.qdot == subjugator::Vector6d::Zero())
{
actionresult.error = "";
actionresult.success = true;
actionserver.setSucceeded(actionresult);
}
}
int main(int argc, char **argv)
{
const double radius = 0.5; // 0.5
const double pitch_step = 0.2;
const int circle_points = 6; // 6
const int circle_rings = 3; // 3
const double plan_time = 5;
const int plan_retry = 1;
//const int pose_id_max = circle_points * 3;
const ros::Duration wait_settle(1.0);
const ros::Duration wait_camera(0.5);
const ros::Duration wait_notreached(3.0);
const ros::Duration wait_reset(30.0);
const std::string planning_group = "bumblebee"; //robot, bumblebee
const std::string end_effector = "tool_flange"; // bumblebee_cam1, tool_flange
const std::string plannerId = "PRMkConfigDefault";
const std::string frame_id = "base_link";
ros::init (argc, argv, "simple_pose_mission");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle;
ros::Publisher cameratate_pub = node_handle.advertise<group4_msgs::PointCloudPose>("/robot_rx60b/camerapose", 5);
ros::Publisher markerPublisher = node_handle.advertise<visualization_msgs::MarkerArray>("/simple_pose_mission/poses", 10);
tf::TransformListener listener;
// Marker
MarkerPose marker("object_center");
addObjectCenterMarker(marker);
// Robot and scene
moveit::planning_interface::MoveGroup group(planning_group);
group.setPlannerId(plannerId);
group.setPoseReferenceFrame(frame_id);
//group.setWorkspace(-1.5,-1.5,1.5,1.5,0,1);
group.setGoalTolerance(0.01f);
group.setPlanningTime(plan_time);
group4_msgs::PointCloudPose msg;
msg.header.frame_id = frame_id;
tf::Pose object_center = marker.getPose("object_center");
NumberedPoses poses = generatePoses(object_center, circle_points, circle_rings, pitch_step, radius);
NumberedPoses::iterator poses_itt;
bool poses_updated = true;
while (ros::ok())
{
bool success;
if (poses_updated || poses_itt == poses.end())
{
poses_itt = poses.begin();
poses_updated = false;
}
NumberedPose numbered_pose = *poses_itt;
visualizePoses(poses, frame_id,markerPublisher, numbered_pose.pose_id);
ROS_INFO("Moving to pose %i of %i", numbered_pose.pose_id, numbered_pose.pose_id_max);
tf::StampedTransform transform;
try{
listener.lookupTransform("/bumblebee_cam1", "/tool_flange",
ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
tf::Pose p = numbered_pose.pose_tf * transform;
geometry_msgs::Pose desired_pose = tfPoseToGeometryPose(p);
group.setPoseTarget(desired_pose, end_effector);
success = false;
for (int itry = 0; itry < plan_retry; itry++)
{
group.setStartStateToCurrentState();
wait_notreached.sleep();
success = group.move();
if (success)
break;
else
ROS_INFO("Plan unsuccessfull.. Retrying! (%i)", itry);
}
if (true)
{
wait_settle.sleep();
geometry_msgs::PoseStamped carmine_pose = group.getCurrentPose("camera_link");
geometry_msgs::PoseStamped bumblebee_pose_left = group.getCurrentPose("bumblebee_cam1");
geometry_msgs::PoseStamped bumblebee_pose_right = group.getCurrentPose("bumblebee_cam2");
msg.header.stamp = ros::Time::now();
msg.pose_id.data = numbered_pose.pose_id;
msg.pose_id_max.data = numbered_pose.pose_id_max;
msg.spin_center_pose = tfPoseToGeometryPose(object_center);
msg.carmine_pose = carmine_pose.pose;
msg.bumblebee_pose_left = bumblebee_pose_left.pose;
msg.bumblebee_pose_right = bumblebee_pose_right.pose;
cameratate_pub.publish(msg);
ROS_INFO("Waiting %f seconds for camera.", wait_camera.toSec());
wait_camera.sleep();
}
if (poses_itt == poses.end())
{
ROS_INFO("Last pose reached. Waiting %f seconds before resetting", wait_reset.toSec());
poses_itt = poses.begin();
wait_reset.sleep();
}
else
poses_itt++;
}
group.stop();
return 0;
}
/**
* \brief Publishes the joint angles for the visualization
*
* \param device0 the robot and its state
*
* \ingroup ROS
*
*/
void publish_joints(struct robot_device* device0){
static int count=0;
static ros::Time t1;
static ros::Time t2;
static ros::Duration d;
if (count == 0){
t1 = t1.now();
}
count ++;
t2 = t2.now();
d = t2-t1;
if (d.toSec()<0.030)
return;
t1=t2;
publish_marker(device0);
sensor_msgs::JointState joint_state;
//update joint_state
joint_state.header.stamp = ros::Time::now();
joint_state.name.resize(28);
joint_state.position.resize(28);
// joint_state.name.resize(14);
// joint_state.position.resize(14);
int left, right;
if (device0->mech[0].type == GOLD_ARM)
{
left = 0;
right = 1;
}
else
{
left = 1;
right = 0;
}
//======================LEFT ARM===========================
joint_state.name[0] ="shoulder_L";
joint_state.position[0] = device0->mech[left].joint[0].jpos + offsets_l.shoulder_off;
joint_state.name[1] ="elbow_L";
joint_state.position[1] = device0->mech[left].joint[1].jpos + offsets_l.elbow_off;
joint_state.name[2] ="insertion_L";
joint_state.position[2] = device0->mech[left].joint[2].jpos + d4 + offsets_l.insertion_off;
joint_state.name[3] ="tool_roll_L";
joint_state.position[3] = device0->mech[left].joint[4].jpos - 45 * d2r + offsets_l.roll_off;
joint_state.name[4] ="wrist_joint_L";
joint_state.position[4] = device0->mech[left].joint[5].jpos + offsets_l.wrist_off;
joint_state.name[5] ="grasper_joint_1_L";
joint_state.position[5] = device0->mech[left].joint[6].jpos + offsets_l.grasp1_off;
joint_state.name[6] ="grasper_joint_2_L";
joint_state.position[6] = device0->mech[left].joint[7].jpos * -1 + offsets_l.grasp2_off;
//======================RIGHT ARM===========================
joint_state.name[7] ="shoulder_R";
joint_state.position[7] = device0->mech[right].joint[0].jpos + offsets_r.shoulder_off;
joint_state.name[8] ="elbow_R";
joint_state.position[8] = device0->mech[right].joint[1].jpos + offsets_r.elbow_off;
joint_state.name[9] ="insertion_R";
joint_state.position[9] = device0->mech[right].joint[2].jpos + d4 + offsets_r.insertion_off;
joint_state.name[10] ="tool_roll_R";
joint_state.position[10] = device0->mech[right].joint[4].jpos + 45 * d2r + offsets_r.roll_off;
joint_state.name[11] ="wrist_joint_R";
joint_state.position[11] = device0->mech[right].joint[5].jpos * -1 + offsets_r.wrist_off;
joint_state.name[12] ="grasper_joint_1_R";
joint_state.position[12] = device0->mech[right].joint[6].jpos + offsets_r.grasp1_off;
joint_state.name[13] ="grasper_joint_2_R";
joint_state.position[13] = device0->mech[right].joint[7].jpos * -1 + offsets_r.grasp2_off;
//======================LEFT ARM===========================
joint_state.name[14] ="shoulder_L2";
joint_state.position[14] = device0->mech[left].joint[0].jpos_d + offsets_l.shoulder_off;
joint_state.name[15] ="elbow_L2";
joint_state.position[15] = device0->mech[left].joint[1].jpos_d + offsets_l.elbow_off;
joint_state.name[16] ="insertion_L2";
joint_state.position[16] = device0->mech[left].joint[2].jpos_d + d4 + offsets_l.insertion_off;
joint_state.name[17] ="tool_roll_L2";
joint_state.position[17] = device0->mech[left].joint[4].jpos_d - 45 * d2r + offsets_l.roll_off;
joint_state.name[18] ="wrist_joint_L2";
joint_state.position[18] = device0->mech[left].joint[5].jpos_d + offsets_l.wrist_off;
joint_state.name[19] ="grasper_joint_1_L2";
joint_state.position[19] = device0->mech[left].joint[6].jpos_d + offsets_l.grasp1_off;
joint_state.name[20] ="grasper_joint_2_L2";
joint_state.position[20] = device0->mech[left].joint[7].jpos_d * -1 + offsets_l.grasp2_off;
//======================RIGHT ARM===========================
joint_state.name[21] ="shoulder_R2";
joint_state.position[21] = device0->mech[right].joint[0].jpos_d + offsets_r.shoulder_off;
joint_state.name[22] ="elbow_R2";
joint_state.position[22] = device0->mech[right].joint[1].jpos_d + offsets_r.elbow_off;
joint_state.name[23] ="insertion_R2";
joint_state.position[23] = device0->mech[right].joint[2].jpos_d + d4 + offsets_r.insertion_off;
joint_state.name[24] ="tool_roll_R2";
joint_state.position[24] = device0->mech[right].joint[4].jpos_d + 45 * d2r + offsets_r.roll_off;
joint_state.name[25] ="wrist_joint_R2";
joint_state.position[25] = device0->mech[right].joint[5].jpos_d * -1 + offsets_r.wrist_off;
joint_state.name[26] ="grasper_joint_1_R2";
joint_state.position[26] = device0->mech[right].joint[6].jpos_d + offsets_r.grasp1_off;
joint_state.name[27] ="grasper_joint_2_R2";
joint_state.position[27] = device0->mech[right].joint[7].jpos_d * -1 + offsets_r.grasp2_off;
//Publish the joint states
joint_publisher.publish(joint_state);
}
bool LWRHWreal::read(ros::Time time, ros::Duration period)
{
// update the robot positions
for (int j = 0; j < LBR_MNJ; j++)
{
joint_position_prev_[j] = joint_position_[j];
joint_position_[j] = device_->getMsrMsrJntPosition()[j];
joint_effort_[j] = device_->getMsrJntTrq()[j];
joint_velocity_[j] = filters::exponentialSmoothing((joint_position_[j]-joint_position_prev_[j])/period.toSec(), joint_velocity_[j], 0.2);
joint_stiffness_[j] = joint_stiffness_command_[j];
}
//this->device_->interface->doDataExchange();
return true;
}
//static void velodyne_callback(const pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::ConstPtr& input)
static void map_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
if (map_loaded == 0) {
std::cout << "Loading map data... ";
map.header.frame_id = "/pointcloud_map_frame";
// Convert the data type(from sensor_msgs to pcl).
pcl::fromROSMsg(*input, map);
pcl::PointCloud<pcl::PointXYZI>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZI>(map));
// Setting point cloud to be aligned to.
ndt.setInputTarget(map_ptr);
// Setting NDT parameters to default values
ndt.setMaximumIterations(iter);
ndt.setResolution(ndt_res);
ndt.setStepSize(step_size);
ndt.setTransformationEpsilon(trans_eps);
map_loaded = 1;
std::cout << "Map Loaded." << std::endl;
}
if (map_loaded == 1 && init_pos_set == 1) {
callback_start = ros::Time::now();
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
tf::Quaternion q_control;
// 1 scan
/*
pcl::PointCloud<pcl::PointXYZI> scan;
pcl::PointXYZI p;
scan.header = input->header;
scan.header.frame_id = "velodyne_scan_frame";
*/
ros::Time scan_time;
scan_time.sec = additional_map.header.stamp / 1000000.0;
scan_time.nsec = (additional_map.header.stamp - scan_time.sec * 1000000.0) * 1000.0;
/*
std::cout << "scan.header.stamp: " << scan.header.stamp << std::endl;
std::cout << "scan_time: " << scan_time << std::endl;
std::cout << "scan_time.sec: " << scan_time.sec << std::endl;
std::cout << "scan_time.nsec: " << scan_time.nsec << std::endl;
*/
t1_start = ros::Time::now();
/*
for (pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::const_iterator item = input->begin(); item != input->end(); item++) {
p.x = (double) item->x;
p.y = (double) item->y;
p.z = (double) item->z;
scan.points.push_back(p);
}
*/
// pcl::fromROSMsg(*input, scan);
t1_end = ros::Time::now();
d1 = t1_end - t1_start;
Eigen::Matrix4f t(Eigen::Matrix4f::Identity());
// pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>(scan));
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_additional_map_ptr(new pcl::PointCloud<pcl::PointXYZI>);
// Downsampling the velodyne scan using VoxelGrid filter
t2_start = ros::Time::now();
pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter;
voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
voxel_grid_filter.setInputCloud(additional_map_ptr);
voxel_grid_filter.filter(*filtered_additional_map_ptr);
t2_end = ros::Time::now();
d2 = t2_end - t2_start;
// Setting point cloud to be aligned.
ndt.setInputSource(filtered_additional_map_ptr);
// Guess the initial gross estimation of the transformation
t3_start = ros::Time::now();
tf::Matrix3x3 init_rotation;
guess_pos.x = previous_pos.x + offset_x;
guess_pos.y = previous_pos.y + offset_y;
guess_pos.z = previous_pos.z + offset_z;
guess_pos.roll = previous_pos.roll;
guess_pos.pitch = previous_pos.pitch;
guess_pos.yaw = previous_pos.yaw + offset_yaw;
Eigen::AngleAxisf init_rotation_x(guess_pos.roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf init_rotation_y(guess_pos.pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf init_rotation_z(guess_pos.yaw, Eigen::Vector3f::UnitZ());
Eigen::Translation3f init_translation(guess_pos.x, guess_pos.y, guess_pos.z);
Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix();
t3_end = ros::Time::now();
d3 = t3_end - t3_start;
t4_start = ros::Time::now();
pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);
ndt.align(*output_cloud, init_guess);
t = ndt.getFinalTransformation();
pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_additional_map_ptr (new pcl::PointCloud<pcl::PointXYZI>());
transformed_additional_map_ptr->header.frame_id = "/map";
pcl::transformPointCloud(*additional_map_ptr, *transformed_additional_map_ptr, t);
sensor_msgs::PointCloud2::Ptr msg_ptr(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*transformed_additional_map_ptr, *msg_ptr);
msg_ptr->header.frame_id = "/map";
ndt_map_pub.publish(*msg_ptr);
// Writing Point Cloud data to PCD file
pcl::io::savePCDFileASCII("global_map.pcd", *transformed_additional_map_ptr);
std::cout << "Saved " << transformed_additional_map_ptr->points.size() << " data points to global_map.pcd." << std::endl;
pcl::PointCloud<pcl::PointXYZRGB> output;
output.width = transformed_additional_map_ptr->width;
output.height = transformed_additional_map_ptr->height;
output.points.resize(output.width * output.height);
for(size_t i = 0; i < output.points.size(); i++){
output.points[i].x = transformed_additional_map_ptr->points[i].x;
output.points[i].y = transformed_additional_map_ptr->points[i].y;
output.points[i].z = transformed_additional_map_ptr->points[i].z;
output.points[i].rgb = 255 << 8;
}
pcl::io::savePCDFileASCII("global_map_rgb.pcd", output);
std::cout << "Saved " << output.points.size() << " data points to global_map_rgb.pcd." << std::endl;
t4_end = ros::Time::now();
d4 = t4_end - t4_start;
t5_start = ros::Time::now();
/*
tf::Vector3 origin;
origin.setValue(static_cast<double>(t(0,3)), static_cast<double>(t(1,3)), static_cast<double>(t(2,3)));
*/
tf::Matrix3x3 tf3d;
tf3d.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)), static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)), static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2)));
// Update current_pos.
current_pos.x = t(0, 3);
current_pos.y = t(1, 3);
current_pos.z = t(2, 3);
tf3d.getRPY(current_pos.roll, current_pos.pitch, current_pos.yaw, 1);
// control_pose
current_pos_control.roll = current_pos.roll;
current_pos_control.pitch = current_pos.pitch;
current_pos_control.yaw = current_pos.yaw - angle / 180.0 * M_PI;
double theta = current_pos_control.yaw;
current_pos_control.x = cos(theta) * (-control_shift_x) + sin(theta) * (-control_shift_y) + current_pos.x;
current_pos_control.y = -sin(theta) * (-control_shift_x) + cos(theta) * (-control_shift_y) + current_pos.y;
current_pos_control.z = current_pos.z - control_shift_z;
// transform "/velodyne" to "/map"
#if 0
transform.setOrigin(tf::Vector3(current_pos.x, current_pos.y, current_pos.z));
q.setRPY(current_pos.roll, current_pos.pitch, current_pos.yaw);
transform.setRotation(q);
#else
//
// FIXME:
// We corrected the angle of "/velodyne" so that pure_pursuit
// can read this frame for the control.
// However, this is not what we want because the scan of Velodyne
// looks unmatched for the 3-D map on Rviz.
// What we really want is to make another TF transforming "/velodyne"
// to a new "/ndt_points" frame and modify pure_pursuit to
// read this frame instead of "/velodyne".
// Otherwise, can pure_pursuit just use "/ndt_frame"?
//
transform.setOrigin(tf::Vector3(current_pos_control.x, current_pos_control.y, current_pos_control.z));
q.setRPY(current_pos_control.roll, current_pos_control.pitch, current_pos_control.yaw);
transform.setRotation(q);
#endif
q_control.setRPY(current_pos_control.roll, current_pos_control.pitch, current_pos_control.yaw);
/*
std::cout << "ros::Time::now(): " << ros::Time::now() << std::endl;
std::cout << "ros::Time::now().sec: " << ros::Time::now().sec << std::endl;
std::cout << "ros::Time::now().nsec: " << ros::Time::now().nsec << std::endl;
*/
// br.sendTransform(tf::StampedTransform(transform, scan_time, "map", "velodyne"));
static tf::TransformBroadcaster pose_broadcaster;
tf::Transform pose_transform;
tf::Quaternion pose_q;
/* pose_transform.setOrigin(tf::Vector3(0, 0, 0));
pose_q.setRPY(0, 0, 0);
pose_transform.setRotation(pose_q);
pose_broadcaster.sendTransform(tf::StampedTransform(pose_transform, scan_time, "map", "ndt_frame"));
*/
// publish the position
// ndt_pose_msg.header.frame_id = "/ndt_frame";
ndt_pose_msg.header.frame_id = "/map";
ndt_pose_msg.header.stamp = scan_time;
ndt_pose_msg.pose.position.x = current_pos.x;
ndt_pose_msg.pose.position.y = current_pos.y;
ndt_pose_msg.pose.position.z = current_pos.z;
ndt_pose_msg.pose.orientation.x = q.x();
ndt_pose_msg.pose.orientation.y = q.y();
ndt_pose_msg.pose.orientation.z = q.z();
ndt_pose_msg.pose.orientation.w = q.w();
static tf::TransformBroadcaster pose_broadcaster_control;
tf::Transform pose_transform_control;
tf::Quaternion pose_q_control;
/* pose_transform_control.setOrigin(tf::Vector3(0, 0, 0));
pose_q_control.setRPY(0, 0, 0);
pose_transform_control.setRotation(pose_q_control);
pose_broadcaster_control.sendTransform(tf::StampedTransform(pose_transform_control, scan_time, "map", "ndt_frame"));
*/
// publish the position
// control_pose_msg.header.frame_id = "/ndt_frame";
control_pose_msg.header.frame_id = "/map";
control_pose_msg.header.stamp = scan_time;
control_pose_msg.pose.position.x = current_pos_control.x;
control_pose_msg.pose.position.y = current_pos_control.y;
control_pose_msg.pose.position.z = current_pos_control.z;
control_pose_msg.pose.orientation.x = q_control.x();
control_pose_msg.pose.orientation.y = q_control.y();
control_pose_msg.pose.orientation.z = q_control.z();
control_pose_msg.pose.orientation.w = q_control.w();
/*
std::cout << "ros::Time::now(): " << ros::Time::now() << std::endl;
std::cout << "ros::Time::now().sec: " << ros::Time::now().sec << std::endl;
std::cout << "ros::Time::now().nsec: " << ros::Time::now().nsec << std::endl;
*/
ndt_pose_pub.publish(ndt_pose_msg);
control_pose_pub.publish(control_pose_msg);
t5_end = ros::Time::now();
d5 = t5_end - t5_start;
#ifdef OUTPUT
// Writing position to position_log.txt
std::ofstream ofs("position_log.txt", std::ios::app);
if (ofs == NULL) {
std::cerr << "Could not open 'position_log.txt'." << std::endl;
exit(1);
}
ofs << current_pos.x << " " << current_pos.y << " " << current_pos.z << " " << current_pos.roll << " " << current_pos.pitch << " " << current_pos.yaw << std::endl;
#endif
// Calculate the offset (curren_pos - previous_pos)
offset_x = current_pos.x - previous_pos.x;
offset_y = current_pos.y - previous_pos.y;
offset_z = current_pos.z - previous_pos.z;
offset_yaw = current_pos.yaw - previous_pos.yaw;
// Update position and posture. current_pos -> previous_pos
previous_pos.x = current_pos.x;
previous_pos.y = current_pos.y;
previous_pos.z = current_pos.z;
previous_pos.roll = current_pos.roll;
previous_pos.pitch = current_pos.pitch;
previous_pos.yaw = current_pos.yaw;
callback_end = ros::Time::now();
d_callback = callback_end - callback_start;
std::cout << "-----------------------------------------------------------------" << std::endl;
std::cout << "Sequence number: " << input->header.seq << std::endl;
std::cout << "Number of scan points: " << additional_map_ptr->size() << " points." << std::endl;
std::cout << "Number of filtered scan points: " << filtered_additional_map_ptr->size() << " points." << std::endl;
std::cout << "NDT has converged: " << ndt.hasConverged() << std::endl;
std::cout << "Fitness score: " << ndt.getFitnessScore() << std::endl;
std::cout << "Number of iteration: " << ndt.getFinalNumIteration() << std::endl;
std::cout << "(x,y,z,roll,pitch,yaw):" << std::endl;
std::cout << "(" << current_pos.x << ", " << current_pos.y << ", " << current_pos.z << ", " << current_pos.roll << ", " << current_pos.pitch << ", " << current_pos.yaw << ")" << std::endl;
std::cout << "Transformation Matrix:" << std::endl;
std::cout << t << std::endl;
#ifdef VIEW_TIME
std::cout << "Duration of velodyne_callback: " << d_callback.toSec() << " secs." << std::endl;
std::cout << "Adding scan points: " << d1.toSec() << " secs." << std::endl;
std::cout << "VoxelGrid Filter: " << d2.toSec() << " secs." << std::endl;
std::cout << "Guessing the initial gross estimation: " << d3.toSec() << " secs." << std::endl;
std::cout << "NDT: " << d4.toSec() << " secs." << std::endl;
std::cout << "tf: " << d5.toSec() << " secs." << std::endl;
#endif
std::cout << "-----------------------------------------------------------------" << std::endl;
}
}
int main(int argc, char **argv) {
// Node setup
ros::init(argc, argv, "quad_node");
ros::NodeHandle n;
ros::NodeHandle priv("~");
prev_time = ros::Time::now();
#ifdef ROVER_1
ROS_ERROR("WHY DOES THIS ROBOT EXIST, AND WHY ARE YOU TESTING CODE ON IT?!");
#endif
// Serial port parameter
std::string port[2];
priv.param("serial_port1", port[0], std::string("/dev/motor_controller1"));
priv.param("serial_port2", port[1], std::string(""));
//priv.param("config_only", configonly, false);
//if no 2nd port is specified, assume it's intentional for testing
if (port[1].size()==0)
NUM_VALID_CONTROLLER_PORTS=1;
// Wheel diameter parameter
priv.param("wheel_diameter", wheel_diameter, 0.3048);
wheel_circumference = wheel_diameter * M_PI;
// Wheel base length
priv.param("robot_width", robot_width, 0.9144);
// Odom Frame id parameter
priv.param("odom_frame_id", odom_frame_id, std::string("odom"));
// Load up some covariances from parameters
priv.param("rotation_covariance",rot_cov, 1.0);
priv.param("position_covariance",pos_cov, 1.0);
priv.param("spam",spam, true);
// Odometry Publisher
odom_pub = n.advertise<nav_msgs::Odometry>("odom", 5);
// Encoder Publisher
encoder_pub = n.advertise<StampedEncoders>("encoders", 5);
// TF Broadcaster
odom_broadcaster = new tf::TransformBroadcaster;
estoppub = n.advertise<std_msgs::Bool>("estopState", 1, true);
std_msgs::Bool b;
b.data = true;
estoppub.publish(b);
voltpub = n.advertise<std_msgs::Float32>("Voltage", 1);
telempub[0] = n.advertise<std_msgs::String>("mc1/telemetry", 1);
telempub[1] = n.advertise<std_msgs::String>("mc2/telemetry", 1);
// cmd_vel Subscriber
ros::Subscriber sub = n.subscribe("cmd_vel", 1, cmd_velCallback);
// raw motor sub for mc[0]
ros::Subscriber rawsub1 = n.subscribe("mc1/raw", 1, raw_callback1);
// raw motor sub for mc[1]
ros::Subscriber rawsub2 = n.subscribe("mc2/raw", 1, raw_callback2);
ros::ServiceServer estopper = n.advertiseService("estop", estop_callback);
ros::ServiceServer sped = n.advertiseService("maxspeed", maxspeed_callback);
ros::ServiceServer spedd = n.advertiseService("speedcoefficient", speedcoefficient);
ros::Subscriber estopsub = n.subscribe("setEstop", 1, estopsub_callback);
while(ros::ok()) {
erroroccurred = false;
for (int i=0;i<NUM_VALID_CONTROLLER_PORTS;i++){
if (configonly && configured[i]) continue;
ROS_INFO("MDC2250[%d] connecting to port %s", i, port[i].c_str());
ROS_INFO("config_only = %s", configonly ? "true" : "false");
try {
mc[i] = new MDC2250();
mc[i]->setExceptionHandler(i==0 ? errorMsgCallback1 : errorMsgCallback2);
mc[i]->setInfoHandler(i==0 ? infoMsgCallback1 : infoMsgCallback2);
// 10000, true
mc[i]->connect(port[i], 10000, true);
if (configonly)
for(int j=1;j<=2;j++)
{
mc[i]->setEncoderPulsesPerRotation(j, ENCODER_CPR*4);
mc[i]->setEncoderUsage(j, mdc2250::constants::feedback, j==1, j==2);
mc[i]->setMaxRPMValue(j, ENCODER_RPM_AT_1000_EFFORT);
mc[i]->setOperatingMode(j, mdc2250::constants::closedloop_speed);
}
if (configonly && !erroroccurred)
{
mc[i]->commitConfig();
configured[i] = true;
}
if (!configonly)
{
init(mc[i]);
}
} catch(ConnectionFailedException &e) {
ROS_ERROR("Failed to connect to the MDC2250[%d]: %s", i, e.what());
erroroccurred = true;
} catch(std::exception &e) {
ROS_ERROR("SOME exception occured while connecting to the MDC2250[%d]: %s", i, e.what());
erroroccurred = true;
}
}
if (!erroroccurred && isConnected())
{
bool estopped = false;
for(int i=0;i<NUM_VALID_CONTROLLER_PORTS;i++)
estopped |= mc[i]->isEstopped();
std_msgs::Bool b;
b.data = estopped;
estoppub.publish(b);
for(int i=0;i<NUM_VALID_CONTROLLER_PORTS;i++)
lasttick[i]=ros::Time::now();
erroroccurred = false;
}
bool conly = configonly;
bool err = erroroccurred;
bool conn = isConnected();
bool ok = ros::ok();
while(!conly && !err && conn && ok) {
ros::spinOnce();
ros::Duration(0.001).sleep();
static ros::Time n = ros::Time::now();
for(int i=0;i<NUM_VALID_CONTROLLER_PORTS;i++)
{
static ros::Duration d = n - lasttick[i];
if (d.toSec() > 0.5)
{
ROS_WARN("TIMED OUT!");
break;
}
}
conly = configonly;
err = erroroccurred;
conn = isConnected();
ok = ros::ok();
}
ROS_WARN("FAILURE REASON: configonly=%s errored=%s !connected=%s !ok=%s",configonly?"true":"false",erroroccurred?"true":"false",!conn?"true":"false",!ok?"true":"false");
for(int i=0;i<NUM_VALID_CONTROLLER_PORTS;i++)
{
if (mc[i] != NULL && mc[i]->isConnected())
{
mc[i]->disconnect();
mc[i] = NULL;
}
}
if((configonly && !erroroccurred) || !ros::ok())
{
break;
}
ROS_INFO("Will try to reconnect to the MCD2250 in 5 seconds.");
ros::Duration(5).sleep();
}
ROS_WARN("Broke out of infinite loop");
return 0;
}
void LWRHWreal::write(ros::Time time, ros::Duration period)
{
static int warning = 0;
for (int j = 0; j < LBR_MNJ; j++)
{
// fake velocity command computed as:
// (desired position - current position) / period, to avoid speed limit error
joint_velocity_command_[j] = (joint_position_command_[j]-joint_position_[j])/period.toSec();
}
// enforce limits
ej_sat_interface_.enforceLimits(period);
ej_limits_interface_.enforceLimits(period);
vj_sat_interface_.enforceLimits(period);
vj_limits_interface_.enforceLimits(period);
pj_sat_interface_.enforceLimits(period);
pj_limits_interface_.enforceLimits(period);
// write to real robot
float newJntPosition[LBR_MNJ];
float newJntStiff[LBR_MNJ];
float newJntDamp[LBR_MNJ];
float newJntAddTorque[LBR_MNJ];
if ( device_->isPowerOn() )
{
// check control mode
//if ( device_->getState() == FRI_STATE_CMD )
//{
// check control scheme
if( device_->getCurrentControlScheme() == FRI_CTRL_JNT_IMP )
{
for (int i = 0; i < LBR_MNJ; i++)
{
newJntPosition[i] = joint_position_command_[i]; // zero for now
newJntAddTorque[i] = joint_effort_command_[i]; // comes from the controllers
newJntStiff[i] = joint_stiffness_command_[i]; // default values for now
newJntDamp[i] = joint_damping_command_[i]; // default values for now
}
// only joint impedance control is performed, since it is the only one that provide access to the joint torque directly
// note that stiffness and damping are 0, as well as the position, since only effort is allowed to be sent
// the KRC adds the dynamic terms, such that if zero torque is sent, the robot apply torques necessary to mantain the robot in the current position
// the only interface is effort, thus any other action you want to do, you have to compute the added torque and send it through a controller
device_->doJntImpedanceControl(newJntPosition, newJntStiff, newJntDamp, newJntAddTorque, true);
}
else if( device_->getCurrentControlScheme() == FRI_CTRL_POSITION )
{
for (int i = 0; i < LBR_MNJ; i++)
{
newJntPosition[i] = joint_position_command_[i];
}
// only joint impedance control is performed, since it is the only one that provide access to the joint torque directly
// note that stiffness and damping are 0, as well as the position, since only effort is allowed to be sent
// the KRC adds the dynamic terms, such that if zero torque is sent, the robot apply torques necessary to mantain the robot in the current position
// the only interface is effort, thus any other action you want to do, you have to compute the added torque and send it through a controller
device_->doPositionControl(newJntPosition, true);
}
else if( this->device_->getCurrentControlScheme() == FRI_CTRL_OTHER ) // Gravity compensation: just read status, but we have to keep FRI alive
{
device_->doDataExchange();
}
//}
}
// Stop request is issued from the other side
/*
if ( this->device_->interface->getFrmKRLInt(0) == -1)
{
ROS_INFO(" Stop request issued from the other side");
this->stop();
}*/
// Quality change leads to output of statistics
// for informational reasons
//
/*if ( this->device_->interface->getQuality() != this->device_->lastQuality )
{
ROS_INFO_STREAM("Quality change detected "<< this->device_->interface->getQuality()<< " \n");
ROS_INFO_STREAM("" << this->device_->interface->getMsrBuf().intf);
this->device_->lastQuality = this->device_->interface->getQuality();
}*/
// this is already done in the doJntImpedance Control setting to true the last flag
// this->device_->interface->doDataExchange();
return;
}