bool DoorTraj::OpenDoor(owd_msgs::OpenDoor::Request &req,
owd_msgs::OpenDoor::Response &res) {
if (req.traj.positions.size() < 2) {
ROS_ERROR_NAMED("OpenDoor","Minimum of 2 traj points required for door-opening trajectory");
res.id = std::string("");
res.ok=false;
res.reason="Minimum of 2 traj points required for door-opening trajectory";
return true;
}
if (req.ee_pose.size() != req.traj.positions.size()) {
ROS_ERROR_NAMED("OpenDoor","Length of ee_pose array (%zd) does not match number of trajectory positions (%zd)",req.ee_pose.size(),req.traj.positions.size());
res.id = std::string("");
res.ok=false;
res.reason="Length of ee_pose array does not match number of trajectory positions";
return true;
}
OWD::TrajType traj;
try {
traj=OWD::Plugin::ros2owd_traj(req.traj);
} catch (const char *error) {
char last_trajectory_error[200];
snprintf(last_trajectory_error,200,"Could not extract valid trajectory: %s",error);
ROS_ERROR_NAMED("OpenDoor","%s",last_trajectory_error);
res.id=std::string("");
res.ok=false;
res.reason=last_trajectory_error;
return true;
}
// compute a new trajectory
try {
DoorTraj *newtraj = new DoorTraj(traj, req.ee_pose, R3(req.pull_direction.x,
req.pull_direction.y,
req.pull_direction.z));
// send it to the arm
if (OWD::Plugin::AddTrajectory(newtraj,res.reason)) {
res.ok=true;
res.id = newtraj->id;
res.reason="";
} else {
delete newtraj;
res.id = std::string("");
res.ok=false;
}
} catch (const char *err) {
res.ok=false;
res.reason=err;
res.id=std::string("");
}
// always return true for the service call so that the client knows that
// the call was processed, as opposed to there being a
// network communication error.
// the client will examine the "ok" field to see if the command was
// actually successful
return true;
}
bool KDLKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int> &redundant_joints)
{
if(num_possible_redundant_joints_ < 0)
{
ROS_ERROR_NAMED("kdl","This group cannot have redundant joints");
return false;
}
if(static_cast<int>(redundant_joints.size()) > num_possible_redundant_joints_)
{
ROS_ERROR_NAMED("kdl","This group can only have %d redundant joints", num_possible_redundant_joints_);
return false;
}
/*
XmlRpc::XmlRpcValue joint_list;
if(private_handle.getParam(group_name+"/redundant_joints", joint_list))
{
ROS_ASSERT(joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
std::vector<std::string> redundant_joints;
for (std::size_t i = 0; i < joint_list.size(); ++i)
{
ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
redundant_joints.push_back(static_cast<std::string>(joint_list[i]));
ROS_INFO_NAMED("kdl","Designated joint: %s as redundant joint", redundant_joints.back().c_str());
}
}
*/
std::vector<unsigned int> redundant_joints_map_index;
unsigned int counter = 0;
for(std::size_t i=0; i < dimension_; ++i)
{
bool is_redundant_joint = false;
for(std::size_t j=0; j < redundant_joints.size(); ++j)
{
if(i == redundant_joints[j])
{
is_redundant_joint = true;
counter++;
break;
}
}
if(!is_redundant_joint)
{
// check for mimic
if(mimic_joints_[i].active)
{
redundant_joints_map_index.push_back(counter);
counter++;
}
}
}
for(std::size_t i=0; i < redundant_joints_map_index.size(); ++i)
ROS_DEBUG_NAMED("kdl","Redundant joint map index: %d %d", (int) i, (int) redundant_joints_map_index[i]);
redundant_joints_map_index_ = redundant_joints_map_index;
redundant_joint_indices_ = redundant_joints;
return true;
}
bool BaseController::init(ros::NodeHandle& nh, ControllerManager* manager)
{
/* We absolutely need access to the controller manager */
if (!manager)
{
ROS_ERROR_NAMED("BaseController", "No controller manager available.");
initialized_ = false;
return false;
}
Controller::init(nh, manager);
/* Initialize joints */
left_ = manager_->getJointHandle("base_l_wheel_joint");
right_ = manager_->getJointHandle("base_r_wheel_joint");
if (left_ == NULL || right_ == NULL)
{
ROS_ERROR_NAMED("BaseController", "Cannot get wheel joints.");
initialized_ = false;
return false;
}
left_last_position_ = left_->getPosition();
right_last_position_ = right_->getPosition();
last_update_ = ros::Time::now();
/* Get base parameters */
nh.param<double>("track_width", track_width_, 0.33665);
nh.param<double>("radians_per_meter", radians_per_meter_, 17.4978147374);
nh.param<bool>("publish_tf", publish_tf_, true);
nh.param<std::string>("odometry_frame", odometry_frame_, "odom");
nh.param<std::string>("base_frame", base_frame_, "base_link");
nh.param<double>("moving_threshold", moving_threshold_, 0.0001);
double t;
nh.param<double>("/timeout", t, 0.25);
timeout_ = ros::Duration(t);
/* Get limits of base controller */
nh.param<double>("max_velocity_x", max_velocity_x_, 1.0);
nh.param<double>("max_velocity_r", max_velocity_r_, 4.5);
nh.param<double>("max_acceleration_x", max_acceleration_x_, 0.75);
nh.param<double>("max_acceleration_r", max_acceleration_r_, 3.0);
/* Subscribe to base commands */
cmd_sub_ = nh.subscribe<geometry_msgs::Twist>("command", 1,
boost::bind(&BaseController::command, this, _1));
/* Publish odometry & tf */
ros::NodeHandle n;
odom_pub_ = n.advertise<nav_msgs::Odometry>("odom", 10);
if (publish_tf_)
broadcaster_.reset(new tf::TransformBroadcaster());
initialized_ = true;
return initialized_;
}
void
Edge_detector_nodelet::callback(const sensor_msgs::ImageConstPtr& input_msg_image){
NODELET_DEBUG("callback");
if(pub_.getNumSubscribers() == 0) return;
cv_bridge::CvImagePtr cv_ptr;
try{
cv_ptr = cv_bridge::toCvCopy(input_msg_image, input_msg_image->encoding);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR_NAMED(node_name_, "cv_bridge exception: %s", e.what());
return;
}
cv::Mat src_gray, dst_gray, dst_color;
if(input_msg_image->encoding == sensor_msgs::image_encodings::MONO8){
src_gray = cv_ptr->image;
}
else{
cvtColor( cv_ptr->image, src_gray, CV_BGR2GRAY );
}
try{
switch(config_.filter){
case 0:
cv::Canny( src_gray, dst_gray, config_.threshold1, config_.threshold2, config_.kernel_size, config_.L2gradient );
break;
case 1:
cv::Laplacian( src_gray, dst_gray, CV_16S, config_.kernel_size, 1 , 0 );
break;
default :
ROS_ERROR_NAMED(node_name_, "Filter not implemented, select filter between 0 and 3:");
}
}catch (cv::Exception &e){
ROS_ERROR_NAMED(node_name_,"cv_bridge exception: %s", e.what());
}
cv_bridge::CvImage image_edge;
if(config_.publish_color){
cvtColor(dst_gray, dst_color, CV_GRAY2BGR);
image_edge = cv_bridge::CvImage(cv_ptr->header, input_msg_image->encoding, dst_color);
}
else{
image_edge = cv_bridge::CvImage(cv_ptr->header, sensor_msgs::image_encodings::MONO8, dst_gray);
}
pub_.publish(image_edge.toImageMsg());
NODELET_DEBUG("callback end");
}
bool read()
{
printf("In:\n");
int msgLenght;
if(initted)
{
mutex.lock();
msgLenght = state_msg->name.size();
if(msgLenght <= 0)
{
ROS_ERROR_NAMED(APPLICATION_NAME, "Received malformed message of size %d", msgLenght);
return false;
}
// size of msg can be smaller than number of joints, but not bigger
if(msgLenght > joint_num)
{
ROS_ERROR_NAMED(APPLICATION_NAME, "Size of input data is %d, which is bigger than numer of joints [%d]", msgLenght, joint_num);
return false;
}
if(state_msg->position.size() != msgLenght)
{
ROS_ERROR_NAMED(APPLICATION_NAME, "Received malformed message. Size of 'names' is different from size of references.");
return false;
}
// same check for other fields?? TBD: define this f*****g msg!!!
// Joints can be in any order, so we need a map to fill in the right spot.
for(int i=0; i<msgLenght; i++)
{
// TODO: using '[]' operator may cause issues, use '.at()' instead
measPos[jointMap[state_msg->name[i]]] = state_msg->position[i];
measVel[jointMap[state_msg->name[i]]] = state_msg->velocity[i];
measEff[jointMap[state_msg->name[i]]] = state_msg->effort[i];
printf("[%2d] %-*s %8.4f\t%8.4f\t%8.4f\n", i, longestJointName_size+2, state_msg->name[i].c_str(), measPos[i], measVel[i], measEff[i]);
}
ROS_WARN_NAMED(APPLICATION_NAME, "TBS: Try to get the most recent reference target maybe??\n");
mutex.unlock();
fflush(stdout);
}
else
{
std::cout << "Not initted yet" << std::endl;
}
return true;
}
bool SrvKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int> &redundant_joints)
{
if(num_possible_redundant_joints_ < 0)
{
ROS_ERROR_NAMED("srv","This group cannot have redundant joints");
return false;
}
if(redundant_joints.size() > num_possible_redundant_joints_)
{
ROS_ERROR_NAMED("srv","This group can only have %d redundant joints", num_possible_redundant_joints_);
return false;
}
return true;
}
void autopilot_version_cb(const ros::TimerEvent &event) {
bool ret = false;
try {
auto client = nh.serviceClient<mavros::CommandLong>("cmd/command");
mavros::CommandLong cmd{};
cmd.request.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
cmd.request.confirmation = false;
cmd.request.param1 = 1.0;
ROS_DEBUG_NAMED("sys", "VER: Sending request.");
ret = client.call(cmd);
}
catch (ros::InvalidNameException &ex) {
ROS_ERROR_NAMED("sys", "VER: %s", ex.what());
}
ROS_ERROR_COND_NAMED(!ret, "sys", "VER: command plugin service call failed!");
if (version_retries > 0) {
version_retries--;
ROS_WARN_COND_NAMED(version_retries != RETRIES_COUNT - 1, "sys",
"VER: request timeout, retries left %d", version_retries);
}
else {
uas->update_capabilities(false);
autopilot_version_timer.stop();
ROS_WARN_NAMED("sys", "VER: your FCU don't support AUTOPILOT_VERSION, "
"switched to default capabilities");
}
}
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.");
}
}
void FeatureTracker::DepthImageCallback(const sensor_msgs::ImageConstPtr &depth_image_msgs)
{
try
{
_cv_ptr_depth = cv_bridge::toCvCopy(depth_image_msgs, sensor_msgs::image_encodings::TYPE_32FC1);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR_NAMED("FeatureTracker.DepthImageCallback", "cv_bridge exception: %s", e.what());
return;
}
//set fix min max values
double min_range_ = 0.5;
double max_range_ = 8.0;
//create cv;;Mat img
cv::Mat img(_cv_ptr_depth->image.rows, _cv_ptr_depth->image.cols, CV_8UC1);
//convert all pixels
for (int i = 0; i < _cv_ptr_depth->image.rows; i++)
{
float* Di = _cv_ptr_depth->image.ptr<float> (i);
char* Ii = img.ptr<char> (i);
for (int j = 0; j < _cv_ptr_depth->image.cols; j++)
{
Ii[j] = (char)(255 * ((Di[j] - min_range_) / (max_range_ - min_range_)));
}
}
// show the image
// cv::imshow(DEPTHWINDOW, img);
// cv::waitKey(1);
}
/*! \brief Sets planning scene based on the stored list of objects and allowed contact region and returns the scene.*/
arm_navigation_msgs::SetPlanningSceneDiff::Response collisionObjects::setPlanningScene(){
arm_navigation_msgs::SetPlanningSceneDiff::Request req;
arm_navigation_msgs::SetPlanningSceneDiff::Response res;
for(unsigned int i=0; i<_att_obj.size(); i++){
if(!_att_obj[i].object.shapes.empty()){
req.planning_scene_diff.attached_collision_objects.push_back(_att_obj[i]);
}
}
for(unsigned int i=0; i<_coll_obj.size(); i++){
if(!_coll_obj[i].shapes.empty()){
req.planning_scene_diff.collision_objects.push_back(_coll_obj[i]);
}
}
if(!_allowed_contact.shape.dimensions.empty()){
req.planning_scene_diff.allowed_contacts.push_back(_allowed_contact);
}
if(!_set_pln_scn_client.call(req, res)){
ROS_ERROR_NAMED(CL_LGRNM,"Call to %s failed", SET_PLANNING_SCENE_DIFF_NAME);
if(!ros::service::exists(SET_PLANNING_SCENE_DIFF_NAME, true)){
ROS_WARN_NAMED(CL_LGRNM,"%s service doesn't exist", SET_PLANNING_SCENE_DIFF_NAME);
}
}
//printListOfObjects();
return res;
}
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 Navigation::approachGoal()
{
move_base_ac_.cancelAllGoals();
move_base_msgs::MoveBaseGoal goal_msg;
switch (goal_) {
case CHARGE:
ROS_INFO_NAMED(name_,"GOING TO CHARGER: %i", BOX_CHARGE);
goal_msg.target_pose.pose = charge_pose_;
break;
case BRICK:
ROS_INFO_NAMED(name_,"GOING TO collect bricks: %i", BRICK);
goal_msg.target_pose.pose = load_bricks_pose_;
break;
case DELIVERY:
ROS_INFO_NAMED(name_,"GOING TO DELIVERY: %i", DELIVERY);
goal_msg.target_pose.pose = delivery_pose_;
break;
case TRANSITION:
ROS_INFO_NAMED(name_,"GOING TO line to manipulators: %i", TRANSITION);
goal_msg.target_pose.pose = line_to_manipulator_pose_;
break;
default:
//ac_.cancelAllGoals();
ROS_ERROR_NAMED(name_,"Unknown behavior type: %i",goal_);
result_.state = free_navigation::NavigateFreelyResult::ERROR;
as_.setAborted(result_, "Unknown behavior type recieved");
return; // not a navigation command so do not navigate
}
sendMoveBaseGoal(goal_msg);
}
void Transforms::setTransform(const Eigen::Isometry3d& t, const std::string& from_frame)
{
if (from_frame.empty())
ROS_ERROR_NAMED("transforms", "Cannot record transform with empty name");
else
transforms_map_[from_frame] = t;
}
void ompl_interface::ConstraintsLibrary::saveConstraintApproximations(const std::string& path)
{
ROS_INFO_NAMED("constraints_library", "Saving %u constrained space approximations to '%s'",
(unsigned int)constraint_approximations_.size(), path.c_str());
try
{
boost::filesystem::create_directories(path);
}
catch (...)
{
}
std::ofstream fout((path + "/manifest").c_str());
if (fout.good())
for (std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.begin();
it != constraint_approximations_.end(); ++it)
{
fout << it->second->getGroup() << std::endl;
fout << it->second->getStateSpaceParameterization() << std::endl;
fout << it->second->hasExplicitMotions() << std::endl;
fout << it->second->getMilestoneCount() << std::endl;
std::string serialization;
msgToHex(it->second->getConstraintsMsg(), serialization);
fout << serialization << std::endl;
fout << it->second->getFilename() << std::endl;
if (it->second->getStateStorage())
it->second->getStateStorage()->store((path + "/" + it->second->getFilename()).c_str());
}
else
ROS_ERROR_NAMED("constraints_library", "Unable to save constraint approximation to '%s'", path.c_str());
fout.close();
}
// start_poseCB Callback
void TOea_Planner::start_poseCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& start_msg)
{
// Astar_.marker_id_ = 0;
Astar_.marker_array_cells_.markers.clear();
if (!map_received_)
{
ROS_ERROR_NAMED(logger_name_, "No map received yet. Unable to compute path.");
return;
}
planner_state.data = hardware::BUSY; //PLANNING;
state_pub_.publish(planner_state);
ROS_DEBUG_NAMED(logger_name_, "New Goal received on topic");
// get world pose from msg
Astar_.robot_init_world_pose_.x = start_msg->pose.pose.position.x;
Astar_.robot_init_world_pose_.y = start_msg->pose.pose.position.y;
Astar_.robot_init_world_pose_.yaw = tf::getYaw(start_msg->pose.pose.orientation);
int x, y, l;
Astar_.ConvertWorlCoordToMatrix(Astar_.robot_init_world_pose_.x, Astar_.robot_init_world_pose_.y, Astar_.robot_init_world_pose_.yaw , x, y, l);
if (mark_end_cubes) //mark start cell
{
Astar_.add_cubes_array(x, y, 0x989898); // start
cells_pub_.publish(Astar_.marker_array_cells_);
}
// print start pose info (coor and cost) to terminal
int index = Astar_.Get_index(l,y,x, "SetGridCellCost(x,y,z)");
std::cout << BOLDMAGENTA << "selected cell is: [l][h][w] = [" << l << "][" << y << "][" << x << "] , with cost: " << + Astar_.AStarMap_.Grid[index].Cost << RESET << std::endl;
}
bool BaseController::start()
{
if (!initialized_)
{
ROS_ERROR_NAMED("BaseController", "Unable to start, not initialized.");
return false;
}
if (ros::Time::now() - last_command_ >= timeout_)
{
ROS_ERROR_NAMED("BaseController", "Unable to start, command has timed out.");
return false;
}
return true;
}
// service to check if some pose is valid
// is valid if in free or high cost zone
// not valid if obstacle, inflated or outside the map
bool TOea_Planner::IsPoseValid(oea_planner::isPoseValid::Request& req, oea_planner::isPoseValid::Response& res)
{
ROS_DEBUG_NAMED(logger_name_, "IsPoseValid Service Called");
if (!map_received_)
{
ROS_ERROR_NAMED(logger_name_, "No map received yet. Can't validate pose!");
return false;
}
int x,y,yaw;
Astar_.ConvertWorlCoordToMatrix(req.x, req.y, req.yaw, x, y, yaw);
int state = Astar_.GetGridCellState(x,y,yaw);
if ((state == AStarObstacle)||(state == AStarInflated)|| (state == AStarInvalid))
{
ROS_DEBUG_NAMED(logger_name_, "Invalid pose");
res.isValid = false;
}
else
{
ROS_DEBUG_NAMED(logger_name_, "Pose is valid");
res.isValid = true;
}
return true;
}
// 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;
}
bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
const std::vector<double> &joint_angles,
std::vector<geometry_msgs::Pose> &poses) const
{
ros::WallTime n1 = ros::WallTime::now();
if(!active_)
{
ROS_ERROR_NAMED("kdl","kinematics not active");
return false;
}
poses.resize(link_names.size());
if(joint_angles.size() != dimension_)
{
ROS_ERROR_NAMED("kdl","Joint angles vector must have size: %d",dimension_);
return false;
}
KDL::Frame p_out;
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
KDL::JntArray jnt_pos_in(dimension_);
for(unsigned int i=0; i < dimension_; i++)
{
jnt_pos_in(i) = joint_angles[i];
}
KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
bool valid = true;
for(unsigned int i=0; i < poses.size(); i++)
{
ROS_DEBUG_NAMED("kdl","End effector index: %d",getKDLSegmentIndex(link_names[i]));
if(fk_solver.JntToCart(jnt_pos_in,p_out,getKDLSegmentIndex(link_names[i])) >=0)
{
tf::poseKDLToMsg(p_out,poses[i]);
}
else
{
ROS_ERROR_NAMED("kdl","Could not compute FK for %s",link_names[i].c_str());
valid = false;
}
}
return valid;
}
bool FollowJointTrajectoryController::start()
{
if (!initialized_)
{
ROS_ERROR_NAMED("FollowJointTrajectoryController",
"Unable to start, not initialized.");
return false;
}
if (!server_->isActive())
{
ROS_ERROR_NAMED("FollowJointTrajectoryController",
"Unable to start, action server is not active.");
return false;
}
return true;
}
void Conversions::fromMsg(const std::vector<graph_slam_msgs::SensorData> &sensor_data, cv::Mat &features)
{
// Count feature data points. Assumption: all have the same feature type.
int feature_type = 0;
int feature_count = 0;
int descriptor_size = 0;
for (auto data : sensor_data) {
if (data.sensor_type == graph_slam_msgs::SensorData::SENSOR_TYPE_FEATURE) {
feature_type = data.features.descriptor_type;
if (data.features.features.size() > 0) {
descriptor_size = data.features.features[0].descriptor.size();
feature_count += data.features.features.size();
}
}
}
if (feature_count > 0) {
int k = 0;
switch (feature_type) {
case graph_slam_msgs::Features::BRIEF:
case graph_slam_msgs::Features::ORB:
case graph_slam_msgs::Features::BRISK:
case graph_slam_msgs::Features::FREAK:
features.create(feature_count, descriptor_size, CV_8U);
for (auto data : sensor_data) {
if (data.sensor_type == graph_slam_msgs::SensorData::SENSOR_TYPE_FEATURE) {
for (unsigned int i = 0; i < data.features.features.size(); i++) {
for (int j = 0; j < descriptor_size; j++) {
float val = data.features.features[i].descriptor[j];
features.at<unsigned char>(k,j) = (unsigned char)val;
}
k++;
}
}
}
break;
case graph_slam_msgs::Features::SURF:
case graph_slam_msgs::Features::SIFT:
features.create(feature_count, descriptor_size, CV_32F);
for (auto data : sensor_data) {
if (data.sensor_type == graph_slam_msgs::SensorData::SENSOR_TYPE_FEATURE) {
for (unsigned int i = 0; i < data.features.features.size(); i++) {
for (int j = 0; j < descriptor_size; j++) {
features.at<float>(k,j) = data.features.features[i].descriptor[j];
}
k++;
}
}
}
break;
default:
ROS_ERROR_NAMED("feature_link_estimation", "unknown feature type: %d (LE)", feature_type);
break;
}
}
}
Transforms::Transforms(const std::string& target_frame) : target_frame_(target_frame)
{
boost::trim(target_frame_);
if (target_frame_.empty())
ROS_ERROR_NAMED("transforms", "The target frame for MoveIt! Transforms cannot be empty.");
else
{
transforms_map_[target_frame_] = Eigen::Isometry3d::Identity();
}
}
/**
* Common function for command service callbacks.
*
* NOTE: success is bool in messages, but has unsigned char type in C++
*/
bool send_command_long_and_wait(uint16_t command, uint8_t confirmation,
float param1, float param2,
float param3, float param4,
float param5, float param6,
float param7,
unsigned char &success, uint8_t &result) {
unique_lock lock(mutex);
/* check transactions */
for (auto it = ack_waiting_list.cbegin();
it != ack_waiting_list.cend(); it++)
if ((*it)->expected_command == command) {
ROS_WARN_THROTTLE_NAMED(10, "cmd", "Command %u alredy in progress", command);
return false;
}
//! @note APM always send COMMAND_ACK, while PX4 never.
bool is_ack_required = (confirmation != 0 || uas->is_ardupilotmega()) && !uas->is_px4();
if (is_ack_required)
ack_waiting_list.push_back(new CommandTransaction(command));
command_long(command, confirmation,
param1, param2,
param3, param4,
param5, param6,
param7);
if (is_ack_required) {
auto it = ack_waiting_list.begin();
for (; it != ack_waiting_list.end(); it++)
if ((*it)->expected_command == command)
break;
if (it == ack_waiting_list.end()) {
ROS_ERROR_NAMED("cmd", "CommandTransaction not found for %u", command);
return false;
}
lock.unlock();
bool is_not_timeout = wait_ack_for(*it);
lock.lock();
success = is_not_timeout && (*it)->result == MAV_RESULT_ACCEPTED;
result = (*it)->result;
delete *it;
ack_waiting_list.erase(it);
}
else {
success = true;
result = MAV_RESULT_ACCEPTED;
}
return true;
}
bool GraspFilter::chooseBestGrasp( const std::vector<moveit_msgs::Grasp>& possible_grasps, moveit_msgs::Grasp& chosen )
{
// TODO: better logic here
if( possible_grasps.empty() )
{
ROS_ERROR_NAMED("filter","There are no grasps to choose from");
return false;
}
chosen = possible_grasps[0]; // just choose first one
return true;
}
bool SimulatedBellowsController::start()
{
if (!initialized_)
{
ROS_ERROR_NAMED("SimulatedBellowsController",
"Unable to start, not initialized.");
return false;
}
return true;
}
bool SrvKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
const std::vector<double> &joint_angles,
std::vector<geometry_msgs::Pose> &poses) const
{
ros::WallTime n1 = ros::WallTime::now();
if(!active_)
{
ROS_ERROR_NAMED("srv","kinematics not active");
return false;
}
poses.resize(link_names.size());
if(joint_angles.size() != dimension_)
{
ROS_ERROR_NAMED("srv","Joint angles vector must have size: %d",dimension_);
return false;
}
ROS_ERROR_STREAM_NAMED("srv","Forward kinematics not implemented");
return false;
}
void BaseController::command(const geometry_msgs::TwistConstPtr& msg)
{
if (!initialized_)
{
ROS_ERROR_NAMED("BaseController", "Unable to accept command, not initialized.");
return;
}
last_command_ = ros::Time::now();
desired_x_ = msg->linear.x;
desired_r_ = msg->angular.z;
manager_->requestStart(name_);
}
SE3 DoorTraj::interpolate_ee_pose(const OWD::JointPos ¤t_pos) {
// use the trajectory position to interpolate between adjacent end-effector poses
while ((current_pose_segment->start_time + current_pose_segment->duration) < time) {
if (++current_pose_segment == pose_segments.end()) {
--current_pose_segment; // back up to the last segment
if (time > current_pose_segment->start_time + current_pose_segment->duration + 0.25) {
ROS_ERROR_NAMED("OpenDoor","Could not find a pose segment for time %2.3f",time);
throw "Could not find a pose segment for current time";
} else {
time = current_pose_segment->start_time + current_pose_segment->duration;
break;
}
}
}
double fraction;
MacQuinticSegment *mqs = dynamic_cast<MacQuinticSegment *>(*current_piece);
if (mqs) {
// if we're in a straight segment, interpolate based on distance
OWD::JointPos progress = current_pos - mqs->start_pos;
fraction = progress.length() / mqs->distance;
} else {
// if we're in a blend, interpolate based on time (since velocity through the blend is
// relatively constant, this is close enough to matching the corresponding position)
fraction = (time - (*current_piece)->start_time) / (*current_piece)->duration;
}
if (fraction < 0) {
ROS_ERROR_NAMED("OpenDoor","Could not find our relative pose position for time %2.3f: fraction=%2.2f",time, fraction);
throw "Could not find our relative pose position: fraction<0";
} else if (fraction > 1) {
ROS_ERROR_NAMED("OpenDoor","Could not find our relative pose position for time %2.3f: fraction=%2.2f",time, fraction);
throw "Could not find our relative pose position: fraction>1";
}
// ROS_DEBUG_STREAM_NAMED("OpenDoor","Current pose segment starts with" << std::endl << current_pose_segment->starting_pose);
// ROS_DEBUG_STREAM_NAMED("OpenDoor","and shifts by " << fraction << " of" << std::endl << current_pose_segment->pose_shift);
SE3 current_pose = current_pose_segment->starting_pose
+ fraction * current_pose_segment->pose_shift;
return current_pose;
}
void Transforms::setTransform(const geometry_msgs::TransformStamped& transform)
{
if (sameFrame(transform.child_frame_id, target_frame_))
{
Eigen::Isometry3d t = tf2::transformToEigen(transform.transform);
setTransform(t, transform.header.frame_id);
}
else
{
ROS_ERROR_NAMED("transforms", "Given transform is to frame '%s', but frame '%s' was expected.",
transform.child_frame_id.c_str(), target_frame_.c_str());
}
}
void safetyarea_cb(const geometry_msgs::PolygonStamped::ConstPtr &req) {
if (req->polygon.points.size() != 2) {
ROS_ERROR_NAMED("safetyarea", "SA: Polygon should contain only two points");
return;
}
send_safety_set_allowed_area(
req->polygon.points[0].x,
req->polygon.points[0].y,
req->polygon.points[0].z,
req->polygon.points[1].x,
req->polygon.points[1].y,
req->polygon.points[1].z);
}
