virtual bool adaptAndPlan(const PlannerFn &planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest &req,
planning_interface::MotionPlanResponse &res,
std::vector<std::size_t> &added_path_index) const
{
ROS_DEBUG("Running '%s'", getDescription().c_str());
// get the specified start state
robot_state::RobotState start_state = planning_scene->getCurrentState();
robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);
// if the start state is otherwise valid but does not meet path constraints
if (planning_scene->isStateValid(start_state, req.group_name) &&
!planning_scene->isStateValid(start_state, req.path_constraints, req.group_name))
{
ROS_INFO("Path constraints not satisfied for start state...");
planning_scene->isStateValid(start_state, req.path_constraints, req.group_name, true);
ROS_INFO("Planning to path constraints...");
planning_interface::MotionPlanRequest req2 = req;
req2.goal_constraints.resize(1);
req2.goal_constraints[0] = req.path_constraints;
req2.path_constraints = moveit_msgs::Constraints();
planning_interface::MotionPlanResponse res2;
// we call the planner for this additional request, but we do not want to include potential
// index information from that call
std::vector<std::size_t> added_path_index_temp;
added_path_index_temp.swap(added_path_index);
bool solved1 = planner(planning_scene, req2, res2);
added_path_index_temp.swap(added_path_index);
if (solved1)
{
planning_interface::MotionPlanRequest req3 = req;
ROS_INFO("Planned to path constraints. Resuming original planning request.");
// extract the last state of the computed motion plan and set it as the new start state
robot_state::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state);
bool solved2 = planner(planning_scene, req3, res);
res.planning_time_ += res2.planning_time_;
if (solved2)
{
// since we add a prefix, we need to correct any existing index positions
for (std::size_t i = 0 ; i < added_path_index.size() ; ++i)
added_path_index[i] += res2.trajectory_->getWayPointCount();
// we mark the fact we insert a prefix path (we specify the index position we just added)
for (std::size_t i = 0 ; i < res2.trajectory_->getWayPointCount() ; ++i)
added_path_index.push_back(i);
// we need to append the solution paths.
res2.trajectory_->append(*res.trajectory_, 0.0);
res2.trajectory_->swap(*res.trajectory_);
return true;
}
else
return false;
}
else
{
ROS_WARN("Unable to plan to path constraints. Running usual motion plan.");
bool result = planner(planning_scene, req, res);
res.planning_time_ += res2.planning_time_;
return result;
}
}
else
{
ROS_DEBUG("Path constraints are OK. Running usual motion plan.");
return planner(planning_scene, req, res);
}
}
void EncoderOdometryNode::publishEstimatedOdomMsg(
const ros::TimerEvent& timer_event) {
// Check to make sure that we have some counts for both the left and right
// encoders
if (!left_encoder_num_ticks_curr || !right_encoder_num_ticks_curr) {
// If we don't have either the left or right encoder ticks, just return
return;
}
// Checks if we've made an estimate before
if (!left_encoder_num_ticks_prev || !right_encoder_num_ticks_prev) {
ROS_DEBUG("Got first ticks from encoders");
// If we haven't, save the current state as the previous state
// and just return. The first actual estimate will be made at the
// next call to this function
left_encoder_num_ticks_prev = left_encoder_num_ticks_curr;
right_encoder_num_ticks_prev = right_encoder_num_ticks_curr;
// Save the time so that next time we get ticks, we can
// generate a state estimate
last_estimate.header.stamp = ros::Time::now();
return;
}
// All math taken from:
// https://www.cs.cmu.edu/afs/cs.cmu.edu/academic/class/16311/www/s07/labs/NXTLabs/Lab%203.html
double dt = ros::Time::now().toSec() - last_estimate.header.stamp.toSec();
// left and right wheel velocities (in meters/s)
int left_encoder_ticks =
*left_encoder_num_ticks_curr - *left_encoder_num_ticks_prev;
int right_encoder_ticks =
*right_encoder_num_ticks_curr - *right_encoder_num_ticks_prev;
double v_l =
(left_encoder_ticks) / dt * (2 * M_PI) / ticks_per_rotation * wheel_radius;
double v_r =
(right_encoder_ticks) / dt * (2 * M_PI) / ticks_per_rotation * wheel_radius;
// linear and angular velocity
double v = (v_r + v_l) / 2;
double w = (v_r - v_l) / wheelbase_length;
// x and y velocity
double x_vel = v * cos(w);
double y_vel = v * sin(w);
// position
double prev_x = last_estimate.pose.pose.position.x;
double prev_y = last_estimate.pose.pose.position.y;
double prev_yaw = tf::getYaw(last_estimate.pose.pose.orientation);
double k_00 = v * cos(prev_yaw);
double k_01 = v * sin(prev_yaw);
double k_02 = w;
double k_10 = v * cos(prev_yaw + dt / 2 * k_02);
double k_11 = v * sin(prev_yaw + dt / 2 * k_02);
double k_12 = w;
double k_20 = v * cos(prev_yaw + dt / 2 * k_12);
double k_21 = v * sin(prev_yaw + dt / 2 * k_12);
double k_22 = w;
double k_30 = v * cos(prev_yaw + dt * k_22);
double k_31 = v * sin(prev_yaw + dt * k_22);
double k_32 = w;
double x = prev_x + (dt / 6) * (k_00 + 2 * (k_10 + k_20) + k_30);
double y = prev_y + (dt / 6) * (k_01 + 2 * (k_11 + k_21) + k_31);
double yaw = prev_yaw + (dt / 6) * (k_02 + 2 * (k_12 + k_22) + k_32);
// Update our estimate
last_estimate.pose.pose.position.x = x;
last_estimate.pose.pose.position.y = y;
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(yaw),
last_estimate.pose.pose.orientation);
last_estimate.twist.twist.linear.x = x_vel;
last_estimate.twist.twist.linear.y = y_vel;
last_estimate.twist.twist.angular.z = w;
last_estimate.header.stamp = ros::Time::now();
last_estimate.header.frame_id = odom_frame_id;
// Save the current number of ticks as the "previous" number for the
// next time we estimate
left_encoder_num_ticks_prev = left_encoder_num_ticks_curr;
right_encoder_num_ticks_prev = right_encoder_num_ticks_curr;
// Publish our estimate
odom_estimate_publisher.publish(last_estimate);
}
// returns true, iff node could be added to the cloud
bool GraphManager::addNode(Node new_node) {
std::clock_t starttime=std::clock();
if(reset_request_) {
int numLevels = 3;
int nodeDistance = 2;
marker_id =0;
time_of_last_transform_= ros::Time();
last_batch_update_=std::clock();
delete optimizer_;
optimizer_ = new AIS::HCholOptimizer3D(numLevels, nodeDistance);
graph_.clear();
matches_.clear();
freshlyOptimized_= false;
reset_request_ = false;
}
if (new_node.feature_locations_2d_.size() <= 5) {
ROS_DEBUG("found only %i features on image, node is not included",(int)new_node.feature_locations_2d_.size());
return false;
}
//set the node id only if the node is actually added to the graph
//needs to be done here as the graph size can change inside this function
new_node.id_ = graph_.size();
//First Node, so only build its index, insert into storage and add a
//vertex at the origin, of which the position is very certain
if (graph_.size()==0) {
new_node.buildFlannIndex(); // create index so that next nodes can use it
// graph_.insert(make_pair(new_node.id_, new_node)); //store
graph_[new_node.id_] = new_node;
optimizer_->addVertex(0, Transformation3(), 1e9*Matrix6::eye(1.0)); //fix at origin
return true;
}
unsigned int num_edges_before = optimizer_->edges().size();
std::vector<cv::DMatch> initial_matches;
ROS_DEBUG("Graphsize: %d", (int) graph_.size());
marker_id = 0; //overdraw old markers
Eigen::Matrix4f ransac_trafo, final_trafo;
bool edge_added_to_base;
std::vector<int> vertices_to_comp = getPotentialEdgeTargets(new_node, -1); //vernetzungsgrad
int id_of_id = vertices_to_comp.size() -1;
for (; id_of_id >=0; id_of_id--) { //go from the back, so the last comparison is with the first node. The last comparison is visualized.
initial_matches = processNodePair(new_node, graph_[vertices_to_comp[id_of_id]],edge_added_to_base,ransac_trafo, final_trafo);
//initial_matches = processNodePair(new_node, graph_rit->second);
//What if the node has not been added? visualizeFeatureFlow3D(graph_rit->second, new_node, initial_matches, matches_, marker_id++);
}
id_of_id++;//set back to last valid id
if (optimizer_->edges().size() > num_edges_before) {
new_node.buildFlannIndex();
graph_[new_node.id_] = new_node;
ROS_DEBUG("Added Node, new Graphsize: %i", (int) graph_.size());
optimizeGraph();
//make the transform of the last node known
ros::TimerEvent unused;
broadcastTransform(unused);
visualizeGraphEdges();
visualizeGraphNodes();
visualizeFeatureFlow3D(graph_[vertices_to_comp[id_of_id]], new_node, initial_matches, matches_, marker_id++);
} else {
ROS_WARN("##### could not find link for PointCloud!");
matches_.clear();
}
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
return (optimizer_->edges().size() > num_edges_before);
}
bool ControllerManager::unloadController(const std::string &name)
{
ROS_DEBUG("Will unload controller '%s'", name.c_str());
// lock the controllers
boost::recursive_mutex::scoped_lock guard(controllers_lock_);
// get reference to controller list
int free_controllers_list = (current_controllers_list_ + 1) % 2;
while (ros::ok() && free_controllers_list == used_by_realtime_){
if (!ros::ok())
return false;
usleep(200);
}
std::vector<ControllerSpec>
&from = controllers_lists_[current_controllers_list_],
&to = controllers_lists_[free_controllers_list];
to.clear();
// Transfers the running controllers over, skipping the one to be removed and the running ones.
bool removed = false;
for (size_t i = 0; i < from.size(); ++i)
{
if (from[i].info.name == name){
if (from[i].c->isRunning()){
to.clear();
ROS_ERROR("Could not unload controller with name %s because it is still running",
name.c_str());
return false;
}
removed = true;
}
else
to.push_back(from[i]);
}
// Fails if we could not remove the controllers
if (!removed)
{
to.clear();
ROS_ERROR("Could not unload controller with name %s because no controller with this name exists",
name.c_str());
return false;
}
// Destroys the old controllers list when the realtime thread is finished with it.
ROS_DEBUG("Realtime switches over to new controller list");
int former_current_controllers_list_ = current_controllers_list_;
current_controllers_list_ = free_controllers_list;
while (ros::ok() && used_by_realtime_ == former_current_controllers_list_){
if (!ros::ok())
return false;
usleep(200);
}
ROS_DEBUG("Destruct controller");
from.clear();
ROS_DEBUG("Destruct controller finished");
ROS_DEBUG("Successfully unloaded controller '%s'", name.c_str());
return true;
}
bool ControllerManager::switchController(const std::vector<std::string>& start_controllers,
const std::vector<std::string>& stop_controllers,
int strictness)
{
if (!stop_request_.empty() || !start_request_.empty())
ROS_FATAL("The switch controller stop and start list are not empty that the beginning of the swithcontroller call. This should not happen.");
if (strictness == 0){
ROS_WARN("Controller Manager: To switch controllers you need to specify a strictness level of controller_manager_msgs::SwitchController::STRICT (%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.",
controller_manager_msgs::SwitchController::Request::STRICT,
controller_manager_msgs::SwitchController::Request::BEST_EFFORT);
strictness = controller_manager_msgs::SwitchController::Request::BEST_EFFORT;
}
ROS_DEBUG("switching controllers:");
for (unsigned int i=0; i<start_controllers.size(); i++)
ROS_DEBUG(" - starting controller %s", start_controllers[i].c_str());
for (unsigned int i=0; i<stop_controllers.size(); i++)
ROS_DEBUG(" - stopping controller %s", stop_controllers[i].c_str());
// lock controllers
boost::recursive_mutex::scoped_lock guard(controllers_lock_);
controller_interface::ControllerBase* ct;
// list all controllers to stop
for (unsigned int i=0; i<stop_controllers.size(); i++)
{
ct = getControllerByName(stop_controllers[i]);
if (ct == NULL){
if (strictness == controller_manager_msgs::SwitchController::Request::STRICT){
ROS_ERROR("Could not stop controller with name %s because no controller with this name exists",
stop_controllers[i].c_str());
stop_request_.clear();
return false;
}
else{
ROS_DEBUG("Could not stop controller with name %s because no controller with this name exists",
stop_controllers[i].c_str());
}
}
else{
ROS_DEBUG("Found controller %s that needs to be stopped in list of controllers",
stop_controllers[i].c_str());
stop_request_.push_back(ct);
}
}
ROS_DEBUG("Stop request vector has size %i", (int)stop_request_.size());
// list all controllers to start
for (unsigned int i=0; i<start_controllers.size(); i++)
{
ct = getControllerByName(start_controllers[i]);
if (ct == NULL){
if (strictness == controller_manager_msgs::SwitchController::Request::STRICT){
ROS_ERROR("Could not start controller with name %s because no controller with this name exists",
start_controllers[i].c_str());
stop_request_.clear();
start_request_.clear();
return false;
}
else{
ROS_DEBUG("Could not start controller with name %s because no controller with this name exists",
start_controllers[i].c_str());
}
}
else{
ROS_DEBUG("Found controller %s that needs to be started in list of controllers",
start_controllers[i].c_str());
start_request_.push_back(ct);
}
}
ROS_DEBUG("Start request vector has size %i", (int)start_request_.size());
// Do the resource management checking
std::list<hardware_interface::ControllerInfo> info_list;
std::vector<ControllerSpec> &controllers = controllers_lists_[current_controllers_list_];
for (size_t i = 0; i < controllers.size(); ++i)
{
bool in_stop_list = false;
for(size_t j = 0; j < stop_request_.size(); j++)
in_stop_list = in_stop_list || (stop_request_[j] == controllers[i].c.get());
bool in_start_list = false;
for(size_t j = 0; j < start_request_.size(); j++)
in_start_list = in_start_list || (start_request_[j] == controllers[i].c.get());
bool add_to_list = controllers[i].c->isRunning();
if (in_stop_list)
add_to_list = false;
if (in_start_list)
add_to_list = true;
if (add_to_list)
info_list.push_back(controllers[i].info);
}
bool in_conflict = robot_hw_->checkForConflict(info_list);
if (in_conflict)
{
//.........这里部分代码省略.........
void DecosSensorRing::ReadSensorRingValues()
{
int r = 1;
if(fd == P_ERROR)
{
ROS_ERROR("Port not opened. Abort!");
return;
}
int totalrx = readData(fd, rxbuf, 64);
rxbuf[totalrx] = 0;
//ROS_INFO("RX: <%s>", rxbuf);
for (int i=0; i<totalrx; i++)
{
unsigned char z = rxbuf[i];
// one frame = [I|%03i|%03i|%03i|%03i|%03i|%03i|U|%04i]
switch (rxstate) // this is our state machine to read one full frame of values
{
case 0:
if (z=='[') rxstate++;
break;
case 1:
if (z=='I')
rxstate++;
else
rxstate = 0;
break;
case 2:
case 4:
case 6:
case 8:
case 10:
case 12:
case 14:
case 16:
if (z=='|')
{
rxstate++;
rxcount = 0;
}
else
rxstate = 0;
break;
case 3:
case 5:
case 7:
case 9:
case 11:
case 13:
if (CollectValue(z, 3)==0) // done with one value
{
int idx = (int)(rxstate-3)/2;
if ((idx>=0) && (idx<6))
sensorSet.IR[idx] = atoi(collectedvalue);
}
break;
case 15:
if (z=='U')
rxstate++;
else
rxstate = 0;
break;
case 17:
if (CollectValue(z, 4)==0) // got value?
{
sensorSet.US = atoi(collectedvalue);
sensorSet.counter++;
sensorSet.valid = true;
rxstate = 0;
}
break;
case 18: // in case a fail occured at step 17 in CollectValue
rxstate = 0;
break;
}
}
if (sensorSet.valid)
{
ROS_DEBUG("Set: %03i, %03i, %03i, %03i, %03i, %03i | %04i", sensorSet.IR[0], sensorSet.IR[1], sensorSet.IR[2], sensorSet.IR[3], sensorSet.IR[4], sensorSet.IR[5], sensorSet.US);
sensorSet.valid = false;
decos_sensorring::SensorValues sv;
sv.IR1 = sensorSet.IR[0];
sv.IR2 = sensorSet.IR[1];
sv.IR3 = sensorSet.IR[2];
sv.IR4 = sensorSet.IR[3];
sv.IR5 = sensorSet.IR[4];
//.........这里部分代码省略.........
/** Dynamic reconfigure callback
*
* Called immediately when callback first defined. Called again
* when dynamic reconfigure starts or changes a parameter value.
*
* @param newconfig new Config values
* @param level bit-wise OR of reconfiguration levels for all
* changed parameters (0xffffffff on initial call)
**/
void Camera1394Driver::reconfig(Config &newconfig, uint32_t level)
{
// Do not run concurrently with poll(). Tell it to stop running,
// and wait on the lock until it does.
reconfiguring_ = true;
boost::mutex::scoped_lock lock(mutex_);
ROS_DEBUG("dynamic reconfigure level 0x%x", level);
// resolve frame ID using tf_prefix parameter
if (newconfig.frame_id == "")
newconfig.frame_id = "camera";
std::string tf_prefix = tf::getPrefixParam(priv_nh_);
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);
if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
{
// must close the device before updating these parameters
closeCamera(); // state_ --> CLOSED
}
if (state_ == Driver::CLOSED)
{
// open with new values
openCamera(newconfig);
}
if (config_.camera_info_url != newconfig.camera_info_url)
{
if (!validateConfig(newconfig))
{
// new URL not valid, use the old one
newconfig.camera_info_url = config_.camera_info_url;
}
}
if (state_ != Driver::CLOSED) // openCamera() succeeded?
{
// configure IIDC features
if (level & Levels::RECONFIGURE_CLOSE)
{
// initialize all features for newly opened device
if (false == dev_->features_->initialize(&newconfig))
{
ROS_ERROR_STREAM("[" << camera_name_
<< "] feature initialization failure");
closeCamera(); // can't continue
}
}
else
{
// update any features that changed
// TODO replace this with a dev_->reconfigure(&newconfig);
dev_->features_->reconfigure(&newconfig);
}
}
config_ = newconfig; // save new parameters
reconfiguring_ = false; // let poll() run again
ROS_DEBUG_STREAM("[" << camera_name_
<< "] reconfigured: frame_id " << newconfig.frame_id
<< ", camera_info_url " << newconfig.camera_info_url);
}
请发表评论