void SplineFitting::readJointTrajFromFile(const std::string& filename, std::size_t num_joints)
{
num_joints_ = num_joints;
// Open file
std::ifstream input_file;
input_file.open(filename.c_str());
// Settings
std::string line;
std::string cell;
// Store data by joints, THEN waypoints
joint_positions_.resize(num_joints_);
// Skip header
std::getline(input_file, line);
bool first_row_read_file = true;
double first_timestamp;
// For each row/trajectory waypoint
while (std::getline(input_file, line))
{
std::stringstream lineStream(line);
// TIME FROM START
if (!std::getline(lineStream, cell, ','))
ROS_ERROR_STREAM_NAMED("csv_to_controller", "no time value");
double timestamp = atof(cell.c_str());
// Make first timestamp zero
if (first_row_read_file)
{
first_timestamp = timestamp;
first_row_read_file = false;
}
timestamps_.push_back(timestamp - first_timestamp);
// For each joint
for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
{
// POSITION
if (!std::getline(lineStream, cell, ','))
ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
joint_positions_[joint_id].push_back(atof(cell.c_str()));
// VELOCITY
if (!std::getline(lineStream, cell, ','))
ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
// UNUSED
// ACCELERATION
if (!std::getline(lineStream, cell, ','))
ROS_ERROR_STREAM_NAMED("csv_to_controller", "no joint value");
// UNUSED
}
} // while
}
bool TransmissionInterfaceLoader::load(const TransmissionInfo& transmission_info)
{
// Create transmission instance
TransmissionPtr transmission;
try
{
TransmissionLoaderPtr transmission_loader = transmission_class_loader_->createInstance(transmission_info.type_);
transmission = transmission_loader->load(transmission_info);
if (!transmission) {return false;}
}
catch(pluginlib::LibraryLoadException &ex)
{
ROS_ERROR_STREAM_NAMED("parser", "Failed to load transmission '" << transmission_info.name_ <<
"'. Unsupported type '" << transmission_info.type_ << "'.\n" << ex.what());
return false;
}
// We currently only deal with transmissions specifying a single hardware interface in the joints
assert(!transmission_info.joints_.empty() && !transmission_info.joints_.front().hardware_interfaces_.empty());
const std::vector<std::string>& hw_ifaces_ref = transmission_info.joints_.front().hardware_interfaces_; // First joint
BOOST_FOREACH(const JointInfo& jnt_info, transmission_info.joints_)
{
// Error out if at least one joint has a different set of hardware interfaces
if (hw_ifaces_ref.size() != jnt_info.hardware_interfaces_.size() ||
!internal::is_permutation(hw_ifaces_ref.begin(), hw_ifaces_ref.end(),
jnt_info.hardware_interfaces_.begin()))
{
ROS_ERROR_STREAM_NAMED("parser",
"Failed to load transmission '" << transmission_info.name_ <<
"'. It has joints with different hardware interfaces. This is currently unsupported.");
return false;
}
}
// Load transmission for all specified hardware interfaces
bool has_at_least_one_hw_iface = false;
BOOST_FOREACH(const std::string& hw_iface, hw_ifaces_ref)
{
RequisiteProviderPtr req_provider;
try
{
req_provider = req_provider_loader_->createInstance(hw_iface);
if (!req_provider) {continue;}
}
catch(pluginlib::LibraryLoadException &ex)
{
ROS_WARN_STREAM_NAMED("parser", "Failed to process the '" << hw_iface <<
"' hardware interface for transmission '" << transmission_info.name_ <<
"'.\n" << ex.what());
continue;
}
const bool load_ok = req_provider->loadTransmissionMaps(transmission_info, loader_data_, transmission);
if (load_ok) {has_at_least_one_hw_iface = true;}
else {continue;}
}
bool DiffDriveController::getWheelNames(ros::NodeHandle& controller_nh,
const std::string& wheel_param,
std::vector<std::string>& wheel_names)
{
XmlRpc::XmlRpcValue wheel_list;
if (!controller_nh.getParam(wheel_param, wheel_list))
{
ROS_ERROR_STREAM_NAMED(name_,
"Couldn't retrieve wheel param '" << wheel_param << "'.");
return false;
}
if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
{
if (wheel_list.size() == 0)
{
ROS_ERROR_STREAM_NAMED(name_,
"Wheel param '" << wheel_param << "' is an empty list");
return false;
}
for (int i = 0; i < wheel_list.size(); ++i)
{
if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_ERROR_STREAM_NAMED(name_,
"Wheel param '" << wheel_param << "' #" << i <<
" isn't a string.");
return false;
}
}
wheel_names.resize(wheel_list.size());
for (int i = 0; i < wheel_list.size(); ++i)
{
wheel_names[i] = static_cast<std::string>(wheel_list[i]);
}
}
else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
{
wheel_names.push_back(wheel_list);
}
else
{
ROS_ERROR_STREAM_NAMED(name_,
"Wheel param '" << wheel_param <<
"' is neither a list of strings nor a string.");
return false;
}
return true;
}
bool endEffectorResponding()
{
if( ee_status_.header.stamp < ros::Time::now() - ros::Duration(1.0) )
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Unable to control end effector: servo status is expired");
return false;
}
if( !ee_status_.alive )
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Unable to control end effector: servo not responding");
return false;
}
return true;
}
void BaxterEffortController::commandCB(const baxter_core_msgs::JointCommandConstPtr& msg)
{
//Check if the number of joints and effort values are equal
if( msg->command.size() != msg->names.size() )
{
ROS_ERROR_STREAM_NAMED("update","List of names does not match list of efforts size, "
<< msg->command.size() << " != " << msg->names.size() );
return;
}
std::map<std::string,std::size_t>::iterator name_it;
// Map incoming joint names and effort values to the correct internal ordering
for(size_t i=0; i<msg->names.size(); i++)
{
// Check if the joint name is in our map
name_it = joint_to_index_map_.find(msg->names[i]);
if( name_it != joint_to_index_map_.end() )
{
// Joint is in the map, so we'll update the joint effort
m.data=msg->command[i];
// Publish the joint effort to the corresponding joint controller
effort_command_pub_[name_it->second].publish( m );
}
}
// Update that new effort values are waiting to be sent to the joints
new_command_ = true;
}
/**
* 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;
};
}
bool computePlan(moveit_msgs::GetMotionPlan::Request& req, moveit_msgs::GetMotionPlan::Response& res)
{
ROS_INFO("Received new planning request...");
if (debug_)
pub_request_.publish(req.motion_plan_request);
planning_interface::MotionPlanResponse response;
ompl_interface::ModelBasedPlanningContextPtr context =
ompl_interface_.getPlanningContext(psm_.getPlanningScene(), req.motion_plan_request);
if (!context)
{
ROS_ERROR_STREAM_NAMED("computePlan", "No planning context found");
return false;
}
context->clear();
bool result = context->solve(response);
if (debug_)
{
if (result)
displaySolution(res.motion_plan_response);
std::stringstream ss;
ROS_INFO("%s", ss.str().c_str());
}
return result;
}
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");
}
void processGoal(const geometry_msgs::Pose& start_block_pose, const geometry_msgs::Pose& end_block_pose )
{
// Change the goal constraints on the servos to be less strict, so that the controllers don't die. this is a hack
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/elbow_pitch_joint/goal", 2); // originally it was 0.45
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/shoulder_pan_joint/goal", 2); // originally it was 0.45
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/wrist_pitch_joint/goal", 2); // originally it was 0.45
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/gripper_roll_joint/goal", 2); // originally it was 0.45
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/wrist_pitch_joint/goal", 2); // originally it was 0.45
if( !pickAndPlace(start_block_pose, end_block_pose) )
{
ROS_ERROR_STREAM_NAMED("pick_place_moveit","Pick and place failed");
if(action_server_.isActive()) // Make sure we haven't sent a fake goal
{
// Report failure
result_.success = false;
action_server_.setSucceeded(result_);
}
}
else
{
if(action_server_.isActive()) // Make sure we haven't sent a fake goal
{
// Report success
result_.success = true;
action_server_.setSucceeded(result_);
}
}
// TODO: remove
ros::shutdown();
}
/*
* 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;
}
// 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;
}
RigidTransformationPtr BodyTrajectory::getRigidTransformation(int frame) const
{
if (frame < this->_time_start || frame > this->getTimeEnd())
{
ROS_ERROR_STREAM_NAMED ("BodyTrajectory.getRigidTransformation",
"Frame " << frame << " is either under " << this->_time_start << " or over " <<this->getTimeEnd());
}
return this->_transformations.at(frame - this->_time_start);
}
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;
}
void SplineFitting::setJointPositions(const std::vector<std::vector<double> >& joint_positions,
const std::vector<double>& timetamps)
{
// Error check
if (joint_positions.empty())
{
ROS_ERROR_STREAM_NAMED(name_, "No waypoints passed in");
return;
}
if (joint_positions.front().empty())
{
ROS_ERROR_STREAM_NAMED(name_, "No joint values passed in");
return;
}
// Copy in num joints
num_joints_ = joint_positions.size();
// Save joint positions
joint_positions_ = joint_positions;
timestamps_ = timetamps;
}
/*
* Implements a synchronous call to the mvn_pln node to move the robot to a specified string location
* Receives:
* location : the name of the map location (goalpoint)
* Returns:
* true : if the robot reach the location
* false : otherwise
*/
bool ServiceManager::mpGetClose(std_msgs::String location)
{
std::string service_name ("/mp_getclose");
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<mvn_pln::mp_getclose>(service_name); //create the service caller
mvn_pln::mp_getclose srv; //create the service and fill it with the parameters
srv.request.location = location;
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: " << location);
return true;
}
else
{
ROS_ERROR_STREAM_NAMED("action_planner", "an error acurred when trying to call the " << service_name << " service with parameters: " << location);
ROS_ERROR_STREAM_NAMED("action_planner", "Error message received from " << service_name << " : " << srv.response.error);
}
return false;
}
bool IMarkerRobotState::setToRandomState(double clearance)
{
static const std::size_t MAX_ATTEMPTS = 1000;
for (std::size_t attempt = 0; attempt < MAX_ATTEMPTS; ++attempt)
{
// Set each planning group to random
for (std::size_t i = 0; i < arm_datas_.size(); ++i)
{
imarker_state_->setToRandomPositions(arm_datas_[i].jmg_);
}
// Update transforms
imarker_state_->update();
planning_scene_monitor::LockedPlanningSceneRO planning_scene(psm_); // Read only lock
// Collision check
// which planning group to collision check, "" is everything
static const bool verbose = false;
if (planning_scene->isStateValid(*imarker_state_, "", verbose))
{
// Check clearance
if (clearance > 0)
{
// which planning group to collision check, "" is everything
if (planning_scene->distanceToCollision(*imarker_state_) < clearance)
{
continue; // clearance is not enough
}
}
ROS_INFO_STREAM_NAMED(name_, "Found valid random robot state after " << attempt << " attempts");
// Set updated pose from robot state
for (std::size_t i = 0; i < arm_datas_.size(); ++i)
end_effectors_[i]->setPoseFromRobotState();
// Send to imarker
for (std::size_t i = 0; i < arm_datas_.size(); ++i)
end_effectors_[i]->sendUpdatedIMarkerPose();
return true;
}
if (attempt == 100)
ROS_WARN_STREAM_NAMED(name_, "Taking long time to find valid random state");
}
ROS_ERROR_STREAM_NAMED(name_, "Unable to find valid random robot state for imarker after " << MAX_ATTEMPTS << " attem"
"pts");
return false;
}
bool OmniDriveController::getWheelNames(ros::NodeHandle& controller_nh,
const std::string& wheel_param,
std::string& wheel_name)
{
XmlRpc::XmlRpcValue wheel_list;
if (!controller_nh.getParam(wheel_param, wheel_list))
{
ROS_ERROR_STREAM_NAMED(name_,
"Couldn't retrieve wheel param '" << wheel_param << "'.");
return false;
}
if (wheel_list.getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_ERROR_STREAM_NAMED(name_,
"Wheel param '" << wheel_param << "' #" <<
" isn't a string.");
return false;
}
wheel_name = static_cast<std::string>(wheel_list);
return true;
}
bool TransmissionInterfaceLoader::load(const std::string& urdf)
{
TransmissionParser parser;
std::vector<TransmissionInfo> infos;
if (!parser.parse(urdf, infos)) {return false;}
if (infos.empty())
{
ROS_ERROR_STREAM_NAMED("parser", "No transmissions were found in the robot description.");
return false;
}
return load(infos);
}
// Recieve Action Goal Function
void processGripperAction(const clam_msgs::ClamGripperCommandGoalConstPtr& goal)
{
goal_ = goal;
if( doGripperAction(goal) )
{
result_.reached_goal = true;
action_server_->setSucceeded(result_);
}
else
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Failed to complete gripper action");
result_.reached_goal = false;
action_server_->setSucceeded(result_);
}
}
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);
*/
}
void rosNamed(const std::vector<std::string> &msgs) {
if (msgs.size()==0) return;
if (msgs.size()==1) { ROS_INFO_STREAM("Kobuki : " << msgs[0]); }
if (msgs.size()==2) {
if (msgs[0] == "debug") { ROS_DEBUG_STREAM("Kobuki : " << msgs[1]); }
else if (msgs[0] == "info" ) { ROS_INFO_STREAM ("Kobuki : " << msgs[1]); }
else if (msgs[0] == "warn" ) { ROS_WARN_STREAM ("Kobuki : " << msgs[1]); }
else if (msgs[0] == "error") { ROS_ERROR_STREAM("Kobuki : " << msgs[1]); }
else if (msgs[0] == "fatal") { ROS_FATAL_STREAM("Kobuki : " << msgs[1]); }
}
if (msgs.size()==3) {
if (msgs[0] == "debug") { ROS_DEBUG_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "info" ) { ROS_INFO_STREAM_NAMED (msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "warn" ) { ROS_WARN_STREAM_NAMED (msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "error") { ROS_ERROR_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
else if (msgs[0] == "fatal") { ROS_FATAL_STREAM_NAMED(msgs[1], "Kobuki : " << msgs[2]); }
}
}
/*
* Implements a call to the prsfnd BB module to perform the pf_find (find a human) command
* Receives:
* personToFind : a string containing the name of the human to find
* timeout : timeout for the bb command
*/
bool ServiceManager::prsfndFind(std::string personToFind, int timeout)
{
std::string service_name ("/pf_find");
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<bbros_bridge::Default_ROS_BB_Bridge>(service_name); //create the service caller
bbros_bridge::Default_ROS_BB_Bridge srv; //create the service and fill it with the parameters
srv.request.parameters = personToFind;
srv.request.timeout = timeout;
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 " << srv.request);
return srv.response.success;
}
else
{
ROS_ERROR_STREAM_NAMED("action_planner", "an error acurred when trying to call the " << service_name << " service with parameters" << srv.request);
}
return false;
}
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;
}
/**
* Send STATUSTEXT messate to rosout with APM severity levels
*
* @param[in] severity APM levels.
*/
void process_statustext_apm_quirk(uint8_t severity, std::string &text) {
switch (severity) {
case 1: // SEVERITY_LOW
ROS_INFO_STREAM_NAMED("fcu", "FCU: " << text);
break;
case 2: // SEVERITY_MEDIUM
ROS_WARN_STREAM_NAMED("fcu", "FCU: " << text);
break;
case 3: // SEVERITY_HIGH
case 4: // SEVERITY_CRITICAL
case 5: // SEVERITY_USER_RESPONSE
ROS_ERROR_STREAM_NAMED("fcu", "FCU: " << text);
break;
default:
ROS_WARN_STREAM_NAMED("fcu", "FCU: UNK(" <<
int(severity) << "): " << text);
break;
};
}
bool IMarkerRobotState::loadFromFile(const std::string& file_name)
{
if (!boost::filesystem::exists(file_name))
{
ROS_WARN_STREAM_NAMED(name_, "File not found: " << file_name);
return false;
}
std::ifstream input_file(file_name);
std::string line;
if (!std::getline(input_file, line))
{
ROS_ERROR_STREAM_NAMED(name_, "Unable to read line");
return false;
}
// Get robot state from file
moveit::core::streamToRobotState(*imarker_state_, line);
return true;
}
/**
* \brief Helper function to decide which, and how many, tip frames a planning group has
* \param jmg - joint model group pointer
* \return tips - list of valid links in a planning group to plan for
*/
std::vector<std::string> chooseTipFrames(const robot_model::JointModelGroup *jmg)
{
std::vector<std::string> tips;
std::map<std::string, std::vector<std::string> >::const_iterator ik_it = iksolver_to_tip_links_.find(jmg->getName());
// Check if tips were loaded onto the rosparam server previously
if (ik_it != iksolver_to_tip_links_.end())
{
// the tip is being chosen based on a corresponding rosparam ik link
ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader","Chooing tip frame of kinematic solver for group "
<< jmg->getName() << " based on values in rosparam server.");
tips = ik_it->second;
}
else
{
// get the last link in the chain
ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader","Chooing tip frame of kinematic solver for group "
<< jmg->getName() << " based on last link in chain");
tips.push_back(jmg->getLinkModels().back()->getName());
}
// Error check
if (tips.empty())
{
ROS_ERROR_STREAM_NAMED("kinematics_plugin_loader","Error choosing kinematic solver tip frame(s).");
}
// Debug tip choices
std::stringstream tip_debug;
tip_debug << "Planning group '" << jmg->getName() << "' has tip(s): ";
for (std::size_t i = 0; i < tips.size(); ++i)
tip_debug << tips[i] << ", ";
ROS_DEBUG_STREAM_NAMED("kinematics_plugin_loader", tip_debug.str());
return tips;
}
bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
double timeout,
std::vector<double> &solution,
const IKCallbackFn &solution_callback,
moveit_msgs::MoveItErrorCodes &error_code,
const std::vector<double> &consistency_limits,
const kinematics::KinematicsQueryOptions &options) const
{
ros::WallTime n1 = ros::WallTime::now();
if(!active_)
{
ROS_ERROR_NAMED("kdl","kinematics not active");
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
if(ik_seed_state.size() != dimension_)
{
ROS_ERROR_STREAM_NAMED("kdl","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
if(!consistency_limits.empty() && consistency_limits.size() != dimension_)
{
ROS_ERROR_STREAM_NAMED("kdl","Consistency limits be empty or must have size " << dimension_ << " instead of size " << consistency_limits.size());
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
KDL::JntArray jnt_seed_state(dimension_);
KDL::JntArray jnt_pos_in(dimension_);
KDL::JntArray jnt_pos_out(dimension_);
KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(), redundant_joint_indices_.size(), position_ik_);
KDL::ChainIkSolverPos_NR_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver_vel, max_solver_iterations_, epsilon_, position_ik_);
ik_solver_vel.setMimicJoints(mimic_joints_);
ik_solver_pos.setMimicJoints(mimic_joints_);
if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
{
ROS_ERROR_NAMED("kdl","Could not set redundant joints");
return false;
}
if(options.lock_redundant_joints)
{
ik_solver_vel.lockRedundantJoints();
}
solution.resize(dimension_);
KDL::Frame pose_desired;
tf::poseMsgToKDL(ik_pose, pose_desired);
ROS_DEBUG_STREAM_NAMED("kdl","searchPositionIK2: Position request pose is " <<
ik_pose.position.x << " " <<
ik_pose.position.y << " " <<
ik_pose.position.z << " " <<
ik_pose.orientation.x << " " <<
ik_pose.orientation.y << " " <<
ik_pose.orientation.z << " " <<
ik_pose.orientation.w);
//Do the IK
for(unsigned int i=0; i < dimension_; i++)
jnt_seed_state(i) = ik_seed_state[i];
jnt_pos_in = jnt_seed_state;
unsigned int counter(0);
while(1)
{
// ROS_DEBUG_NAMED("kdl","Iteration: %d, time: %f, Timeout: %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
counter++;
if(timedOut(n1,timeout))
{
ROS_DEBUG_NAMED("kdl","IK timed out");
error_code.val = error_code.TIMED_OUT;
ik_solver_vel.unlockRedundantJoints();
return false;
}
int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
ROS_DEBUG_NAMED("kdl","IK valid: %d", ik_valid);
if(!consistency_limits.empty())
{
getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
if( (ik_valid < 0 && !options.return_approximate_solution) || !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
{
ROS_DEBUG_NAMED("kdl","Could not find IK solution: does not match consistency limits");
continue;
}
}
else
{
getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
ROS_DEBUG_NAMED("kdl","New random configuration");
for(unsigned int j=0; j < dimension_; j++)
ROS_DEBUG_NAMED("kdl","%d %f", j, jnt_pos_in(j));
if(ik_valid < 0 && !options.return_approximate_solution)
{
ROS_DEBUG_NAMED("kdl","Could not find IK solution");
continue;
}
}
ROS_DEBUG_NAMED("kdl","Found IK solution");
for(unsigned int j=0; j < dimension_; j++)
solution[j] = jnt_pos_out(j);
if(!solution_callback.empty())
solution_callback(ik_pose,solution,error_code);
else
error_code.val = error_code.SUCCESS;
if(error_code.val == error_code.SUCCESS)
{
ROS_DEBUG_STREAM_NAMED("kdl","Solved after " << counter << " iterations");
ik_solver_vel.unlockRedundantJoints();
return true;
}
}
ROS_DEBUG_NAMED("kdl","An IK that satisifes the constraints and is collision free could not be found");
error_code.val = error_code.NO_IK_SOLUTION;
ik_solver_vel.unlockRedundantJoints();
return false;
}
// Open end effector
bool openEndEffector()
{
// Error check - servos are alive and we've been recieving messages
if( !endEffectorResponding() )
{
return false;
}
// Check if end effector is already open and arm is still
if( ee_status_.target_position == END_EFFECTOR_OPEN_VALUE_MAX &&
ee_status_.moving == false &&
ee_status_.position > END_EFFECTOR_OPEN_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < END_EFFECTOR_OPEN_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE )
{
// Consider the ee to already be in the corret position
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector open: already in position");
return true;
}
// Set the velocity for the end effector servo
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Setting end effector servo velocity");
velocity_client_ = nh_.serviceClient< dynamixel_hardware_interface::SetVelocity >(EE_VELOCITY_SRV_NAME);
while(!velocity_client_.waitForExistence(ros::Duration(10.0)))
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
return false;
}
dynamixel_hardware_interface::SetVelocity set_velocity_srv;
set_velocity_srv.request.velocity = END_EFFECTOR_VELOCITY;
if( !velocity_client_.call(set_velocity_srv) )
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
return false;
}
// Publish command to servos
std_msgs::Float64 joint_value;
joint_value.data = END_EFFECTOR_OPEN_VALUE_MAX;
end_effector_pub_.publish(joint_value);
// Wait until end effector is done moving
int timeout = 0;
while( ee_status_.moving == true &&
ee_status_.position > END_EFFECTOR_OPEN_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < END_EFFECTOR_OPEN_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE &&
ros::ok() )
{
// Feedback
feedback_.position = ee_status_.position;
//TODO: fill in more of the feedback
action_server_->publishFeedback(feedback_);
// Looping
ros::Duration(0.25).sleep();
++timeout;
if( timeout > 16 ) // wait 4 seconds
{
ROS_ERROR_NAMED("clam_gripper_controller","Unable to open end effector: timeout on goal position");
return false;
}
}
// It worked!
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Finished end effector action");
return true;
}
// Cancel the action
void preemptCB()
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Action prempted - NOT IMPLEMENTED");
// set the action state to preempted
action_server_->setPreempted();
}
bool DiffDriveController::setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
const std::string& left_wheel_name,
const std::string& right_wheel_name,
bool lookup_wheel_separation,
bool lookup_wheel_radius)
{
if (!(lookup_wheel_separation || lookup_wheel_radius))
{
// Short-circuit in case we don't need to look up anything, so we don't have to parse the URDF
return true;
}
// Parse robot description
const std::string model_param_name = "robot_description";
bool res = root_nh.hasParam(model_param_name);
std::string robot_model_str="";
if (!res || !root_nh.getParam(model_param_name,robot_model_str))
{
ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server.");
return false;
}
boost::shared_ptr<urdf::ModelInterface> model(urdf::parseURDF(robot_model_str));
boost::shared_ptr<const urdf::Joint> left_wheel_joint(model->getJoint(left_wheel_name));
boost::shared_ptr<const urdf::Joint> right_wheel_joint(model->getJoint(right_wheel_name));
if (lookup_wheel_separation)
{
// Get wheel separation
if (!left_wheel_joint)
{
ROS_ERROR_STREAM_NAMED(name_, left_wheel_name
<< " couldn't be retrieved from model description");
return false;
}
if (!right_wheel_joint)
{
ROS_ERROR_STREAM_NAMED(name_, right_wheel_name
<< " couldn't be retrieved from model description");
return false;
}
ROS_INFO_STREAM("left wheel to origin: " << left_wheel_joint->parent_to_joint_origin_transform.position.x << ","
<< left_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
<< left_wheel_joint->parent_to_joint_origin_transform.position.z);
ROS_INFO_STREAM("right wheel to origin: " << right_wheel_joint->parent_to_joint_origin_transform.position.x << ","
<< right_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
<< right_wheel_joint->parent_to_joint_origin_transform.position.z);
wheel_separation_ = euclideanOfVectors(left_wheel_joint->parent_to_joint_origin_transform.position,
right_wheel_joint->parent_to_joint_origin_transform.position);
}
if (lookup_wheel_radius)
{
// Get wheel radius
if (!getWheelRadius(model->getLink(left_wheel_joint->child_link_name), wheel_radius_))
{
ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << left_wheel_name << " wheel radius");
return false;
}
}
return true;
}
