bool EKF::doPredict(double dt) {
ROS_DEBUG_NAMED("ekf.prediction", "EKF prediction (dt = %f):", dt);
ROS_DEBUG_STREAM_NAMED("ekf.prediction", "A = [" << std::endl << A << "]");
ROS_DEBUG_STREAM_NAMED("ekf.prediction", "Q = [" << std::endl << Q << "]");
#ifdef USE_HECTOR_TIMING
{ hector_diagnostics::TimingSection section("predict.ekf.covariance");
#endif
state().P() = A * state().P() * A.transpose() + Q;
#ifdef USE_HECTOR_TIMING
}
{ hector_diagnostics::TimingSection section("predict.ekf.state");
#endif
state().update(x_diff);
#ifdef USE_HECTOR_TIMING
}
#endif
ROS_DEBUG_STREAM_NAMED("ekf.prediction", "x_pred = [" << state().getVector().transpose() << "]");
ROS_DEBUG_STREAM_NAMED("ekf.prediction", "P_pred = [" << std::endl << state().getCovariance() << "]");
Filter::doPredict(dt);
return true;
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
GazeboRosLaser::~GazeboRosLaser()
{
ROS_DEBUG_STREAM_NAMED("gpu_laser","Shutting down GPU Laser");
this->rosnode_->shutdown();
delete this->rosnode_;
ROS_DEBUG_STREAM_NAMED("gpu_laser","Unloaded");
}
bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Isometry3d poses,
const moveit::core::JointModelGroup* group)
{
std::vector<std::string> tips;
for (std::size_t i = 0; i < arm_datas_.size(); ++i)
tips.push_back(arm_datas_[i].ee_link_->getName());
// ROS_DEBUG_STREAM_NAMED(name_, "First pose should be for joint model group: " << arm_datas_[0].ee_link_->getName());
const double timeout = 1.0 / 30.0; // 30 fps
// Optionally collision check
moveit::core::GroupStateValidityCallbackFn constraint_fn;
if (true)
{
bool collision_checking_verbose_ = false;
bool only_check_self_collision_ = false;
// TODO(davetcoleman): this is currently not working, the locking seems to cause segfaults
// TODO(davetcoleman): change to std shared_ptr
boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(psm_));
constraint_fn = boost::bind(&isIKStateValid, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get(),
collision_checking_verbose_, only_check_self_collision_, visual_tools_, _1, _2, _3);
}
// Solve
std::size_t outer_attempts = 20;
for (std::size_t i = 0; i < outer_attempts; ++i)
{
if (!imarker_state_->setFromIK(group, poses, tips, timeout, constraint_fn))
{
ROS_DEBUG_STREAM_NAMED(name_, "Failed to find dual arm pose, trying again");
// Re-seed
imarker_state_->setToRandomPositions(group);
}
else
{
ROS_DEBUG_STREAM_NAMED(name_, "Found IK solution");
// Visualize robot
publishRobotState();
// Update the imarkers
for (IMarkerEndEffectorPtr ee : end_effectors_)
ee->setPoseFromRobotState();
return true;
}
}
ROS_ERROR_STREAM_NAMED(name_, "Failed to find dual arm pose");
return false;
}
RayTraceIterRange rayTrace (const nm::MapMetaData& info, const gm::Point& p1, const gm::Point& p2,
bool project_onto_grid, bool project_source_onto_grid,
float max_range)
{
gm::Point np2 = p2;
if (max_range > 0) {
double distance = euclideanDistance(p1,p2);
if (distance > max_range) {
np2.x = ((p2.x - p1.x) * max_range/distance) + p1.x;
np2.y = ((p2.y - p1.y) * max_range/distance) + p1.y;
}
}
Cell c1 = pointCell(info, p1);
Cell c2 = pointCell(info, np2);
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Ray tracing between " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
const RayTraceIterator done(c1, c1, true);
const RayTraceIterRange empty_range(done, done);
if (!withinBounds(info, c1)) {
if (project_source_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c2, c1);
if (c)
c1 = *c;
else
return empty_range;
}
else
throw PointOutOfBoundsException(p1);
}
if (!withinBounds(info, np2)) {
if (project_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c1, c2);
if (c)
c2 = *c;
else
return empty_range;
}
else {
throw PointOutOfBoundsException(np2);
}
}
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Projected ray trace endpoints to " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
return rayTrace(c1, c2);
}
void maxMinLimits()
{
// just a simple loop going from min to max joint limits then resetting
for (std::size_t i = 0; i < 15; i++)
{
double delta = (joint_limits_[i][1] - joint_limits_[i][0]) / 25.0;
ROS_DEBUG_STREAM_NAMED("maxMinLimits","delta = " << delta);
planning_msg_.position[i] += delta;
if (planning_msg_.position[i] > joint_limits_[i][1])
{
planning_msg_.position[i] = joint_limits_[i][0];
}
ROS_DEBUG_STREAM_NAMED("maxMinLimits","position = " << i << ", value = " << planning_msg_.position[i]);
}
}
void GenericHWInterface::loadURDF(ros::NodeHandle &nh, std::string param_name)
{
std::string urdf_string;
urdf_model_ = new urdf::Model();
// search and wait for robot_description on param server
while (urdf_string.empty() && ros::ok())
{
std::string search_param_name;
if (nh.searchParam(param_name, search_param_name))
{
ROS_INFO_STREAM_NAMED("generic_hw_interface", "Waiting for model URDF on the ROS param server at location: " <<
nh.getNamespace() << search_param_name);
nh.getParam(search_param_name, urdf_string);
}
else
{
ROS_INFO_STREAM_NAMED("generic_hw_interface", "Waiting for model URDF on the ROS param server at location: " <<
nh.getNamespace() << param_name);
nh.getParam(param_name, urdf_string);
}
usleep(100000);
}
if (!urdf_model_->initString(urdf_string))
ROS_ERROR_STREAM_NAMED("generic_hw_interface", "Unable to load URDF model");
else
ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Received URDF from param server");
}
bool QuadrotorPropulsion::processQueue(const ros::Time ×tamp, const ros::Duration &tolerance, const ros::Duration &delay, const ros::WallDuration &wait, ros::CallbackQueue *callback_queue) {
boost::mutex::scoped_lock lock(command_queue_mutex_);
bool new_command = false;
ros::Time min(timestamp), max(timestamp);
try {
min = timestamp - delay - tolerance /* - ros::Duration(dt) */;
} catch (std::runtime_error &e) {}
try {
max = timestamp - delay + tolerance;
} catch (std::runtime_error &e) {}
do {
while(!command_queue_.empty()) {
hector_uav_msgs::MotorPWMConstPtr new_motor_voltage = command_queue_.front();
ros::Time new_time = new_motor_voltage->header.stamp;
if (new_time.isZero() || (new_time >= min && new_time <= max)) {
setVoltage(*new_motor_voltage);
command_queue_.pop();
new_command = true;
// new motor command is too old:
} else if (new_time < min) {
ROS_DEBUG_NAMED("quadrotor_propulsion", "Command received was %fs too old. Discarding.", (new_time - timestamp).toSec());
command_queue_.pop();
// new motor command is too new:
} else {
break;
}
}
if (command_queue_.empty() && !new_command) {
if (!motor_status_.on || wait.isZero()) break;
ROS_DEBUG_NAMED("quadrotor_propulsion", "Waiting for command at simulation step t = %fs... last update was %fs ago", timestamp.toSec(), (timestamp - last_command_time_).toSec());
if (!callback_queue) {
if (command_condition_.timed_wait(lock, wait.toBoost())) continue;
} else {
lock.unlock();
callback_queue->callAvailable(wait);
lock.lock();
if (!command_queue_.empty()) continue;
}
ROS_ERROR_NAMED("quadrotor_propulsion", "Command timed out. Disabled motors.");
shutdown();
}
} while(false);
if (new_command) {
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Using motor command valid at t = " << last_command_time_.toSec() << "s for simulation step at t = " << timestamp.toSec() << "s (dt = " << (timestamp - last_command_time_).toSec() << "s)");
}
return new_command;
}
void RobotState::DVLCallback(const underwater_sensor_msgs::DVLConstPtr &message)
{
cv::Point3f white_noise(var_nor(),var_nor(),var_nor());
float elapsed_time;
if(message->bi_error < 1)
{
vel_dvl_.x = message->bi_x_axis;
vel_dvl_.y = message->bi_y_axis;
vel_dvl_.z = message->bi_z_axis;
//vel_dvl_ += white_noise;
//ROS_DEBUG_STREAM_NAMED("rs","DVL velocity = " << vel_dvl_);
if(has_dvl_ && has_imu_)
{
elapsed_time = (message->header.stamp - timestamp_).toSec();
computeTraveledDistances(elapsed_time);
}
timestamp_ = message->header.stamp;
has_dvl_ = true;
}
else
{
ROS_DEBUG_STREAM_NAMED("rs","O erro da DVL eh " << message->bi_error);
}
}
void ControlDoneCB(const actionlib::SimpleClientGoalState& state, const oea_controller::controlPlatformResultConstPtr &result)
{
controlling_ = false;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Control Action finished: " << state.toString());
move_result_.result_state = result->result_state;
move_result_.error_string = result->error_string;
if (move_result_.result_state)
{
as_.setSucceeded(move_result_);
ROS_INFO_NAMED(logger_name_, "Goal was successful :)");
}
else
{
ROS_WARN_NAMED(logger_name_, "Goal was NOT successful :)");
// if is preempted => as_ was already set, cannot set again
if (state.toString() != "PREEMPTED")
{
as_.setAborted(move_result_);
ROS_DEBUG_NAMED(logger_name_, "Goal was Aborted");
}
else
{
if (set_terminal_state_)
{
as_.setPreempted(move_result_);
ROS_DEBUG_NAMED(logger_name_, "Goal was Preempted");
}
}
}
}
void initialize(UAS &uas_,
ros::NodeHandle &nh,
diagnostic_updater::Updater &diag_updater)
{
bool pose_with_covariance;
bool listen_tf;
uas = &uas_;
sp_nh = ros::NodeHandle(nh, "position");
sp_nh.param("vision/pose_with_covariance", pose_with_covariance, false);
sp_nh.param("vision/listen_tf", listen_tf, false);
sp_nh.param<std::string>("vision/frame_id", frame_id, "local_origin");
sp_nh.param<std::string>("vision/child_frame_id", child_frame_id, "vision");
sp_nh.param("vision/tf_rate_limit", tf_rate, 50.0);
ROS_DEBUG_STREAM_NAMED("position", "Vision pose topic type: " <<
((pose_with_covariance)? "PoseWithCovarianceStamped" : "PoseStamped"));
if (listen_tf) {
ROS_INFO_STREAM_NAMED("position", "Listen to vision transform " << frame_id
<< " -> " << child_frame_id);
tf_start("VisionTF", &VisionPoseEstimatePlugin::send_vision_transform);
}
else if (pose_with_covariance)
vision_sub = sp_nh.subscribe("vision", 10, &VisionPoseEstimatePlugin::vision_cov_cb, this);
else
vision_sub = sp_nh.subscribe("vision", 10, &VisionPoseEstimatePlugin::vision_cb, this);
}
void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t level)
{
ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved.");
// Set the gains
setGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min);
}
void testIKServiceCall()
{
// define goal pose for right hand end effector
Eigen::Affine3d goal_pose = Eigen::Affine3d::Identity();
goal_pose *= Eigen::AngleAxisd(3.141593, Eigen::Vector3d::UnitY());
goal_pose.translation()[0] = 0.6;
goal_pose.translation()[1] = -0.5;
goal_pose.translation()[2] = 0.1;
visual_tools_->publishAxisLabeled(goal_pose, "goal_pose");
// call ik service
geometry_msgs::PoseStamped tf2_msg;
tf2_msg.header.stamp = ros::Time::now();
tf::poseEigenToMsg(goal_pose, tf2_msg.pose);
ik_srv_.request.pose_stamp.push_back(tf2_msg);
// TODO: not the right way to call this? compile error on this section...
if (ik_client_right_.call(ik_srv_))
{
ROS_DEBUG_STREAM_NAMED("testIKServiceCall","ik service called successfully.");
}
else
{
ROS_WARN_STREAM_NAMED("testIKServiceCall","Failed to call ik service...");
}
}
/*
* Implements a synchronous call to the mvn_pln node to move the robot a specified distance and angle
* Receives:
* bearing : the angle the robot must turn
* distance : the distance the robot must move
* traveledBearing : the angle the robot actually turn after perform the command (reference)
* traveledDistance : the distance the robot actually moves after perform the command (reference)
* Returns:
* true : if the robot performs correctly the move action
* false : otherwise
*/
bool ServiceManager::mpMove(std_msgs::Float32 bearing, std_msgs::Float32 distance, std_msgs::Float32 &traveledBearing, std_msgs::Float32 &traveledDistance)
{
std::string service_name ("/mp_move_dist_angle");
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<mvn_pln::mp_move_dist_angle>(service_name); //create the service caller
mvn_pln::mp_move_dist_angle srv; //create the service and fill it with the parameters
srv.request.bearing = bearing;
srv.request.distance = distance;
if(client.call(srv)) //call the service with the parameters contained in srv
{
ROS_DEBUG_STREAM_NAMED("action_planner", service_name << " service called successfully with parameters (bearing, distance) = (" << bearing << ", " << distance << ")");
traveledBearing = srv.response.traveledBearing;
traveledDistance = srv.response.traveledDistance;
return true;
}
else
{
ROS_ERROR_STREAM_NAMED("action_planner", "an error acurred when trying to call the " << service_name << " service with parameters (bearing, distance) = (" << bearing << ", " << distance << ")");
traveledBearing = srv.response.traveledBearing;
traveledDistance = srv.response.traveledDistance;
}
return false;
}
/**
* Sent STATUSTEXT message to rosout
*
* @param[in] severity Levels defined in common.xml
*/
void process_statustext_normal(uint8_t severity, std::string &text) {
switch (severity) {
case MAV_SEVERITY_EMERGENCY:
case MAV_SEVERITY_ALERT:
case MAV_SEVERITY_CRITICAL:
case MAV_SEVERITY_ERROR:
ROS_ERROR_STREAM_NAMED("fcu", "FCU: " << text);
break;
case MAV_SEVERITY_WARNING:
case MAV_SEVERITY_NOTICE:
ROS_WARN_STREAM_NAMED("fcu", "FCU: " << text);
break;
case MAV_SEVERITY_INFO:
ROS_INFO_STREAM_NAMED("fcu", "FCU: " << text);
break;
case MAV_SEVERITY_DEBUG:
ROS_DEBUG_STREAM_NAMED("fcu", "FCU: " << text);
break;
default:
ROS_WARN_STREAM_NAMED("fcu", "FCU: UNK(" <<
int(severity) << "): " << text);
break;
};
}
// Get the URDF XML from the parameter server
std::string GazeboRosControlPlugin::getURDF(std::string param_name) const
{
std::string urdf_string;
// search and wait for robot_description on param server
while (urdf_string.empty())
{
std::string search_param_name;
if (model_nh_.searchParam(param_name, search_param_name))
{
ROS_INFO_ONCE_NAMED("gazebo_ros_control", "gazebo_ros_control plugin is waiting for model"
" URDF in parameter [%s] on the ROS param server.", search_param_name.c_str());
model_nh_.getParam(search_param_name, urdf_string);
}
else
{
ROS_INFO_ONCE_NAMED("gazebo_ros_control", "gazebo_ros_control plugin is waiting for model"
" URDF in parameter [%s] on the ROS param server.", robot_description_.c_str());
model_nh_.getParam(param_name, urdf_string);
}
usleep(100000);
}
ROS_DEBUG_STREAM_NAMED("gazebo_ros_control", "Recieved urdf from param server, parsing...");
return urdf_string;
}
void DiffDriveController::cmdVelCallback(const geometry_msgs::Twist& command)
{
if (isRunning())
{
// check that we don't have multiple publishers on the command topic
if (!allow_multiple_cmd_vel_publishers_ && sub_command_.getNumPublishers() > 1)
{
ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << sub_command_.getNumPublishers()
<< " publishers. Only 1 publisher is allowed. Going to brake.");
brake();
return;
}
command_struct_.ang = command.angular.z;
command_struct_.lin = command.linear.x;
command_struct_.stamp = ros::Time::now();
command_.writeFromNonRT (command_struct_);
ROS_DEBUG_STREAM_NAMED(name_,
"Added values to command. "
<< "Ang: " << command_struct_.ang << ", "
<< "Lin: " << command_struct_.lin << ", "
<< "Stamp: " << command_struct_.stamp);
}
else
{
ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
}
}
//Constructor
BaxterControlTest(int test)
: nh_("~")
{
ROS_DEBUG_STREAM_NAMED("constructor","starting test " << test);
visual_tools_.reset(new rviz_visual_tools::RvizVisualTools("base", "visual_tools"));
visual_tools_->deleteAllMarkers();
void setJointLimts();
void initializePlanningMsg();
// Set up subs/pubs
seq_ = 0;
planning_state_pub_ = nh_.advertise<sensor_msgs::JointState>("my_states", 20);
// Subsribe to block poses
block_pose_sub_ = nh_.subscribe("/finger_sensor_test/blockpose", 1, &BaxterControlTest::callback, this );
// Set up services
ik_client_right_ = nh_.serviceClient<baxter_core_msgs::SolvePositionIK>("/ExternalTools/right/PositionKinematicsNode/IKService");
testIKServiceCall();
while (ros::ok())
{
}
}
// Get the URDF XML from the parameter server
std::string getURDF(ros::NodeHandle &model_nh_, std::string param_name)
{
std::string urdf_string;
std::string robot_description = "/robot_description";
// search and wait for robot_description on param server
while (urdf_string.empty())
{
std::string search_param_name;
if (model_nh_.searchParam(param_name, search_param_name))
{
ROS_INFO_ONCE_NAMED("LWRHWFRIL", "LWRHWFRIL node is waiting for model"
" URDF in parameter [%s] on the ROS param server.", search_param_name.c_str());
model_nh_.getParam(search_param_name, urdf_string);
}
else
{
ROS_INFO_ONCE_NAMED("LWRHWFRIL", "LWRHWFRIL node is waiting for model"
" URDF in parameter [%s] on the ROS param server.", robot_description.c_str());
model_nh_.getParam(param_name, urdf_string);
}
usleep(100000);
}
ROS_DEBUG_STREAM_NAMED("LWRHWFRIL", "Recieved urdf from param server, parsing...");
return urdf_string;
}
bool doGripperAction(const clam_msgs::ClamGripperCommandGoalConstPtr& goal)
{
if( goal_->position == clam_msgs::ClamGripperCommandGoal::GRIPPER_OPEN )
{
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Received open end effector goal");
if( simulation_mode_ )
{
// Publish command to servos
std_msgs::Float64 joint_value;
joint_value.data = END_EFFECTOR_OPEN_VALUE_MAX;
end_effector_pub_.publish(joint_value);
return true;
}
else
{
return openEndEffector();
}
}
else if( goal_->position == clam_msgs::ClamGripperCommandGoal::GRIPPER_CLOSE )
{
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Received close end effector goal");
if( simulation_mode_ )
{
// Publish command to servos
std_msgs::Float64 joint_value;
joint_value.data = END_EFFECTOR_OPEN_VALUE_MAX * 0.80;
end_effector_pub_.publish(joint_value);
return true;
}
else
{
return closeEndEffector();
}
}
else
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Unrecognized command " << goal_->position);
}
/*
case clam_msgs::ClamGripperCommandGoal::END_EFFECTOR_SET:
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Received close end effector to setpoint goal");
setEndEffector(goal_->end_effector_setpoint);
*/
}
// Close end effector to setpoint
bool setEndEffector(double setpoint)
{
// Error check - servos are alive and we've been recieving messages
if( !endEffectorResponding() )
{
return false;
}
// Check that there is a valid end effector setpoint set
if( setpoint >= END_EFFECTOR_CLOSE_VALUE_MAX &&
setpoint <= END_EFFECTOR_OPEN_VALUE_MAX )
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Unable to set end effector: out of range setpoint of " <<
setpoint << ". Valid range is " << END_EFFECTOR_CLOSE_VALUE_MAX << " to "
<< END_EFFECTOR_OPEN_VALUE_MAX );
return false;
}
// Check if end effector is already close and arm is still
if( ee_status_.target_position == setpoint &&
ee_status_.moving == false &&
ee_status_.position > setpoint + END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < setpoint - END_EFFECTOR_POSITION_TOLERANCE )
{
// Consider the ee to already be in the corret position
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector close: already in position");
return true;
}
// Publish command to servos
std_msgs::Float64 joint_value;
joint_value.data = setpoint;
end_effector_pub_.publish(joint_value);
// Wait until end effector is done moving
int timeout = 0;
while( ee_status_.moving == true &&
ee_status_.position > setpoint + END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < setpoint - END_EFFECTOR_POSITION_TOLERANCE &&
ros::ok() )
{
// Feedback
feedback_.position = ee_status_.position;
//TODO: fill in more of the feedback
action_server_->publishFeedback(feedback_);
ros::Duration(0.25).sleep();
++timeout;
if( timeout > 16 ) // wait 4 seconds
{
ROS_ERROR_NAMED("clam_gripper_controller","Unable to close end effector: timeout on goal position");
return false;
}
}
return true;
}
// Constructor
GraspFilter::GraspFilter( robot_state::RobotState robot_state,
moveit_visual_tools::VisualToolsPtr& visual_tools ):
robot_state_(robot_state),
visual_tools_(visual_tools),
verbose_(false)
{
ROS_DEBUG_STREAM_NAMED("filter","Loaded simple grasp filter");
}
void QuadrotorPropulsion::update(double dt)
{
if (dt <= 0.0) return;
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.twist: " << PrintVector<double>(propulsion_model_->u.begin(), propulsion_model_->u.begin() + 6));
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.voltage: " << PrintVector<double>(propulsion_model_->u.begin() + 6, propulsion_model_->u.begin() + 10));
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.x_prior: " << PrintVector<double>(propulsion_model_->x.begin(), propulsion_model_->x.end()));
checknan(propulsion_model_->u, "propulsion model input");
checknan(propulsion_model_->x, "propulsion model state");
// update propulsion model
f(propulsion_model_->x.data(), propulsion_model_->u.data(), dt, propulsion_model_->y.data(), propulsion_model_->x_pred.data());
propulsion_model_->x = propulsion_model_->x_pred;
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.x_post: " << PrintVector<double>(propulsion_model_->x.begin(), propulsion_model_->x.end()));
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.force: " << PrintVector<double>(propulsion_model_->y.begin() + 0, propulsion_model_->y.begin() + 3) << " " <<
"propulsion.torque: " << PrintVector<double>(propulsion_model_->y.begin() + 3, propulsion_model_->y.begin() + 6));
checknan(propulsion_model_->y, "propulsion model output");
wrench_.force.x = propulsion_model_->y[0];
wrench_.force.y = -propulsion_model_->y[1];
wrench_.force.z = propulsion_model_->y[2];
wrench_.torque.x = propulsion_model_->y[3];
wrench_.torque.y = -propulsion_model_->y[4];
wrench_.torque.z = -propulsion_model_->y[5];
motor_status_.voltage[0] = propulsion_model_->u[6];
motor_status_.voltage[1] = propulsion_model_->u[7];
motor_status_.voltage[2] = propulsion_model_->u[8];
motor_status_.voltage[3] = propulsion_model_->u[9];
motor_status_.frequency[0] = propulsion_model_->y[6];
motor_status_.frequency[1] = propulsion_model_->y[7];
motor_status_.frequency[2] = propulsion_model_->y[8];
motor_status_.frequency[3] = propulsion_model_->y[9];
motor_status_.running = motor_status_.frequency[0] > 1.0 && motor_status_.frequency[1] > 1.0 && motor_status_.frequency[2] > 1.0 && motor_status_.frequency[3] > 1.0;
motor_status_.current[0] = propulsion_model_->y[10];
motor_status_.current[1] = propulsion_model_->y[11];
motor_status_.current[2] = propulsion_model_->y[12];
motor_status_.current[3] = propulsion_model_->y[13];
}
void UrHardwareInterface::init() {
ROS_INFO_STREAM_NAMED("ur_hardware_interface",
"Reading rosparams from namespace: " << nh_.getNamespace());
// Get joint names
nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0) {
ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
"No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace());
exit(-1);
}
num_joints_ = joint_names_.size();
// Resize vectors
joint_position_.resize(num_joints_);
joint_velocity_.resize(num_joints_);
joint_effort_.resize(num_joints_);
joint_position_command_.resize(num_joints_);
joint_velocity_command_.resize(num_joints_);
// Initialize controller
for (std::size_t i = 0; i < num_joints_; ++i) {
ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
"Loading joint name: " << joint_names_[i]);
// Create joint state interface
joint_state_interface_.registerHandle(
hardware_interface::JointStateHandle(joint_names_[i],
&joint_position_[i], &joint_velocity_[i],
&joint_effort_[i]));
// Create position joint interface
position_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_position_command_[i]));
// Create velocity joint interface
velocity_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_velocity_command_[i]));
}
// Create force torque interface
force_torque_interface_.registerHandle(
hardware_interface::ForceTorqueSensorHandle("wrench", "",
robot_force_, robot_torque_));
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&position_joint_interface_); // From RobotHW base class.
registerInterface(&velocity_joint_interface_); // From RobotHW base class.
registerInterface(&force_torque_interface_); // From RobotHW base class.
velocity_interface_running_ = false;
position_interface_running_ = false;
}
void QuadrotorAerodynamics::update(double dt)
{
if (dt <= 0.0) return;
boost::mutex::scoped_lock lock(mutex_);
// fill input vector u for drag model
drag_model_->u[0] = (twist_.linear.x - wind_.x);
drag_model_->u[1] = -(twist_.linear.y - wind_.y);
drag_model_->u[2] = -(twist_.linear.z - wind_.z);
drag_model_->u[3] = twist_.angular.x;
drag_model_->u[4] = -twist_.angular.y;
drag_model_->u[5] = -twist_.angular.z;
// We limit the input velocities to +-100. Required for numeric stability in case of collisions,
// where velocities returned by Gazebo can be very high.
limit(drag_model_->u, -100.0, 100.0);
// convert input to body coordinates
Eigen::Quaterniond orientation(orientation_.w, orientation_.x, orientation_.y, orientation_.z);
Eigen::Matrix<double,3,3> rotation_matrix(orientation.toRotationMatrix());
Eigen::Map<Eigen::Vector3d> linear(&(drag_model_->u[0]));
Eigen::Map<Eigen::Vector3d> angular(&(drag_model_->u[3]));
linear = rotation_matrix * linear;
angular = rotation_matrix * angular;
ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.twist: " << PrintVector<double>(drag_model_->u.begin(), drag_model_->u.begin() + 6));
checknan(drag_model_->u, "drag model input");
// update drag model
f(drag_model_->u.data(), dt, drag_model_->y.data());
ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.force: " << PrintVector<double>(drag_model_->y.begin() + 0, drag_model_->y.begin() + 3));
ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.torque: " << PrintVector<double>(drag_model_->y.begin() + 3, drag_model_->y.begin() + 6));
checknan(drag_model_->y, "drag model output");
// drag_model_ gives us inverted vectors!
wrench_.force.x = -( drag_model_->y[0]);
wrench_.force.y = -(-drag_model_->y[1]);
wrench_.force.z = -(-drag_model_->y[2]);
wrench_.torque.x = -( drag_model_->y[3]);
wrench_.torque.y = -(-drag_model_->y[4]);
wrench_.torque.z = -(-drag_model_->y[5]);
}
void orientationCallback(const geometry_msgs::Vector3 msg)
{
base_orientation_[0] = msg.x * 3.14159 / 180.0;
base_orientation_[1] = msg.y * 3.14159 / 180.0;
base_orientation_[2] = msg.z * 3.14159 / 180.0;
ROS_DEBUG_STREAM_NAMED("ik_test","orientation: " << base_orientation_[0] << ", " <<
base_orientation_[1] << ", " << base_orientation_[2]);
calculateArmLengths();
}
void planningDoneCB(const actionlib::SimpleClientGoalState& state, const oea_planner::planResultConstPtr &result)
{
planning_ = false;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Plan Action finished: " << state.toString());
move_result_.result_state = result->result_state;
if (move_result_.result_state) //if plan OK
{
planned_path_goal_.plan_goal = result->planned_path; // goal for the controller is result of the planner
if (ctrl_state_sub.getNumPublishers()==0)
{
ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - Controller is down");
controlling_ = false;
move_result_.result_state = 0;
move_result_.error_string = "Controller is down!!!";
as_.setAborted(move_result_);
}
else
{
ac_control_.sendGoal(planned_path_goal_, boost::bind(&MovePlatformAction::ControlDoneCB, this, _1, _2),
actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleActiveCallback(),
actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleFeedbackCallback()); //boost::bind(&MovePlatformAction::ControlFeedbackCB, this,_1));
controlling_ = true;
ROS_DEBUG_STREAM_NAMED(logger_name_,"Goal #" << move_goal_.nav_goal.header.seq << " sent to Controller");
}
}
else //if plan NOT OK
{
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
move_result_.error_string = "Planning Failed: " + result->error_string;
ROS_WARN_STREAM_NAMED(logger_name_, "Aborting because " << move_result_.error_string);
as_.setAborted(move_result_);
}
return;
}
IMarkerRobotState::IMarkerRobotState(planning_scene_monitor::PlanningSceneMonitorPtr psm,
const std::string& imarker_name, std::vector<ArmData> arm_datas,
rviz_visual_tools::colors color, const std::string& package_path)
: name_(imarker_name), nh_("~"), psm_(psm), arm_datas_(arm_datas), color_(color), package_path_(package_path)
{
// Load Visual tools
visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
psm_->getRobotModel()->getModelFrame(), nh_.getNamespace() + "/" + imarker_name, psm_);
// visual_tools_->setPlanningSceneMonitor(psm_);
visual_tools_->loadRobotStatePub(nh_.getNamespace() + "/imarker_" + imarker_name + "_state");
// Load robot state
imarker_state_ = std::make_shared<moveit::core::RobotState>(psm_->getRobotModel());
imarker_state_->setToDefaultValues();
// Create Marker Server
const std::string imarker_topic = nh_.getNamespace() + "/" + imarker_name + "_imarker";
imarker_server_ = std::make_shared<interactive_markers::InteractiveMarkerServer>(imarker_topic, "", false);
// Get file name
if (!getFilePath(file_path_, "imarker_" + name_ + ".csv", "config/imarkers"))
exit(-1);
// Load previous pose from file
if (!loadFromFile(file_path_))
ROS_INFO_STREAM_NAMED(name_, "Unable to find state from file, setting to default");
// Show initial robot state loaded from file
publishRobotState();
// Create each end effector
end_effectors_.resize(arm_datas_.size());
for (std::size_t i = 0; i < arm_datas_.size(); ++i)
{
std::string eef_name;
if (i == 0)
eef_name = imarker_name + "_right";
else
eef_name = imarker_name + "_left";
end_effectors_[i] = std::make_shared<IMarkerEndEffector>(this, eef_name, arm_datas_[i], color);
// Create map from eef name to object
name_to_eef_[eef_name] = end_effectors_[i];
}
// After both end effectors have been added, apply on server
imarker_server_->applyChanges();
ROS_DEBUG_STREAM_NAMED(name_, "IMarkerRobotState '" << name_ << "' Ready.");
}
void QuadrotorPropulsion::addPWMToQueue(const hector_uav_msgs::MotorPWMConstPtr& pwm)
{
boost::mutex::scoped_lock lock(command_queue_mutex_);
if (!motor_status_.on) {
ROS_WARN_NAMED("quadrotor_propulsion", "Received new motor command. Enabled motors.");
engage();
}
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Received motor command valid at " << pwm->header.stamp);
command_queue_.push(pwm);
command_condition_.notify_all();
}
bool sendServoCommand(const double angles[6])
{
// create message header
ROS_DEBUG_STREAM_NAMED("ik_test","sending command to servo...");
trajectory_msg_.header.seq = id_++;
trajectory_msg_.header.stamp = ros::Time::now();
trajectory_msg_.header.frame_id = "/world";
std::vector<std::string> joint_names;
joint_names.push_back("one");
joint_names.push_back("two");
joint_names.push_back("three");
joint_names.push_back("four");
joint_names.push_back("five");
joint_names.push_back("six");
trajectory_msg_.joint_names = joint_names;
trajectory_msgs::JointTrajectoryPoint joint_trajectory_point_msg;
std::vector<trajectory_msgs::JointTrajectoryPoint> points;
std::vector<double> positions;
std::vector<double> velocities;
std::vector<double> accelerations;
std::vector<double> effort;
for (std::size_t i = 0; i < 6; i++)
{
positions.push_back(angles[i]);
velocities.push_back(0.1);
accelerations.push_back(0);
effort.push_back(0);
}
ros::Duration time_from_start = ros::Duration(2.0);
joint_trajectory_point_msg.positions = positions;
joint_trajectory_point_msg.velocities = velocities;
joint_trajectory_point_msg.accelerations = accelerations;
joint_trajectory_point_msg.effort = effort;
joint_trajectory_point_msg.time_from_start = time_from_start;
points.push_back(joint_trajectory_point_msg);
trajectory_msg_.points = points;
// publish message
servo_pub_.publish(trajectory_msg_);
ros::spinOnce();
return true;
}
RayTraceIterRange rayTrace (const nm::MapMetaData& info, const gm::Point& p1, const gm::Point& p2,
bool project_onto_grid, bool project_source_onto_grid)
{
Cell c1 = pointCell(info, p1);
Cell c2 = pointCell(info, p2);
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Ray tracing between " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
const RayTraceIterator done(c1, c1, true);
const RayTraceIterRange empty_range(done, done);
if (!withinBounds(info, c1)) {
if (project_source_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c2, c1);
if (c)
c1 = *c;
else
return empty_range;
}
else
throw PointOutOfBoundsException(p1);
}
if (!withinBounds(info, p2)) {
if (project_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c1, c2);
if (c)
c2 = *c;
else
return empty_range;
}
else {
throw PointOutOfBoundsException(p2);
}
}
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Projected ray trace endpoints to " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
return rayTrace(c1, c2);
}
