void OPTCalibrationNode::spin()
{
ros::Rate rate(5.0);
while (ros::ok())
{
ros::spinOnce();
calibration_->nextAcquisition();
int count = 0;
try
{
#pragma omp parallel for
for (size_t i = 0; i < pinhole_vec_.size(); ++i)
{
const PinholeRGBDevice::Ptr & device = pinhole_vec_[i];
if (device->hasNewMessages())
{
device->convertLastMessages();
PinholeRGBDevice::Data::Ptr data = device->lastData();
OPTCalibration::CheckerboardView::Ptr cb_view;
ROS_DEBUG_STREAM("[" << device->frameId() << "] analysing image generated at: " << device->lastMessages().image_msg->header.stamp);
#pragma omp critical
if (calibration_->analyzeData(device->sensor(), data->image, cb_view))
{
calibration_->addData(device->sensor(), cb_view);
images_acquired_map_[device->frameId()]++;
ROS_INFO_STREAM("[" << device->frameId() << "] checkerboard detected");
++count;
}
}
}
for (size_t i = 0; i < kinect_vec_.size(); ++i)
{
const KinectDevice::Ptr & device = kinect_vec_[i];
if (device->hasNewMessages())
{
device->convertLastMessages();
KinectDevice::Data::Ptr data = device->lastData();
OPTCalibration::CheckerboardView::Ptr color_cb_view;
OPTCalibration::CheckerboardView::Ptr depth_cb_view;
ROS_DEBUG_STREAM("[" << device->colorFrameId() << "] analysing image generated at: " << device->lastMessages().image_msg->header.stamp);
ROS_DEBUG_STREAM("[" << device->depthFrameId() << "] analysing cloud generated at: " << device->lastMessages().cloud_msg->header.stamp);
if (calibration_->analyzeData(device->colorSensor(), device->depthSensor(),
data->image, data->cloud,
color_cb_view, depth_cb_view))
{
calibration_->addData(device->colorSensor(), color_cb_view);
calibration_->addData(device->depthSensor(), depth_cb_view);
images_acquired_map_[device->colorFrameId()]++;
ROS_INFO_STREAM("[" << device->colorFrameId() << "] checkerboard detected");
++count;
}
}
}
for (size_t i = 0; i < swiss_ranger_vec_.size(); ++i)
{
const SwissRangerDevice::Ptr & device = swiss_ranger_vec_[i];
if (device->hasNewMessages())
{
device->convertLastMessages();
SwissRangerDevice::Data::Ptr data = device->lastData();
OPTCalibration::CheckerboardView::Ptr color_cb_view;
OPTCalibration::CheckerboardView::Ptr depth_cb_view;
ROS_DEBUG_STREAM("[" << device->frameId() << "] analysing image generated at: " << device->lastMessages().intensity_msg->header.stamp);
ROS_DEBUG_STREAM("[" << device->frameId() << "] analysing cloud generated at: " << device->lastMessages().cloud_msg->header.stamp);
if (calibration_->analyzeData(device->intensitySensor(), device->depthSensor(),
data->intensity_image, data->cloud,
color_cb_view, depth_cb_view))
{
calibration_->addData(device->intensitySensor(), color_cb_view);
calibration_->addData(device->depthSensor(), depth_cb_view);
images_acquired_map_[device->frameId()]++;
ROS_INFO_STREAM("[" << device->frameId() << "] checkerboard detected");
++count;
}
}
}
}
catch (cv_bridge::Exception & ex)
{
ROS_ERROR_STREAM("cv_bridge exception: " << ex.what());
return;
}
catch (std::runtime_error & ex)
{
ROS_ERROR_STREAM("exception: " << ex.what());
return;
}
status_msg_.header.stamp = ros::Time::now();
status_msg_.header.seq++;
for (size_t i = 0; i < status_msg_.sensor_ids.size(); ++i)
{
status_msg_.images_acquired[i] = images_acquired_map_[status_msg_.sensor_ids[i]];
}
status_pub_.publish(status_msg_);
calibration_->perform();
calibration_->publish();
if (count > 0)
ROS_INFO("-----------------------------------------------");
rate.sleep();
}
}
/** 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);
}
/** 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 reconfig(Config &newconfig, uint32_t level)
{
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(privNH_);
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
if (openCamera(newconfig))
{
// update camera name string
newconfig.camera_name = camera_name_;
}
}
///THIS IS A QUICK HACK TO GET EXPOSURE ON OUR CAMERA'S THIS DOES NOT WORK FOR ALL CAMERAS
if(config_.exposure != newconfig.exposure){
try {
cam_->set_control(0x9a0901, newconfig.exposure);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting exposure. Exception was " << e.what());
}
}
if(config_.absolute_exposure != newconfig.absolute_exposure){
try {
cam_->set_control(0x9a0902, newconfig.absolute_exposure);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting absolute exposure. Exception was " << e.what());
}
}
if(config_.sharpness != newconfig.sharpness){
try {
cam_->set_control(0x98091b, newconfig.sharpness);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting sharpness. Exception was " << e.what());
}
}
if(config_.power_line_frequency != newconfig.power_line_frequency){
try {
cam_->set_control(0x980918, newconfig.power_line_frequency);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting powerline frequency. Exception was " << e.what());
}
}
if(config_.white_balance_temperature != newconfig.white_balance_temperature){
try {
cam_->set_control(0x98090c, newconfig.white_balance_temperature);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting white balance temperature. Exception was " << e.what());
}
}
if(config_.gain != newconfig.gain){
try {
cam_->set_control(0x980913, newconfig.gain);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting gain. Exception was " << e.what());
}
}
if(config_.saturation != newconfig.saturation){
try {
cam_->set_control(0x980902, newconfig.saturation);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting saturation. Exception was " << e.what());
}
}
if(config_.contrast != newconfig.contrast){
try {
cam_->set_control(0x980901, newconfig.contrast);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting contrast. Exception was " << e.what());
}
}
if(config_.brightness != newconfig.brightness){
try {
cam_->set_control(0x980900, newconfig.brightness);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting brightness. Exception was " << e.what());
}
}
if (config_.camera_info_url != newconfig.camera_info_url)
{
// set the new URL and load CameraInfo (if any) from it
if (cinfo_.validateURL(newconfig.camera_info_url))
{
cinfo_.loadCameraInfo(newconfig.camera_info_url);
}
else
{
// 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
// dev_->features_->reconfigure(&newconfig);
// }
// }
config_ = newconfig; // save new parameters
ROS_DEBUG_STREAM("[" << camera_name_
<< "] reconfigured: frame_id " << newconfig.frame_id
<< ", camera_info_url " << newconfig.camera_info_url);
}
void GraphManager::visualizeFeatureFlow3D(const Node& earlier_node,
const Node& newer_node,
const std::vector<cv::DMatch>& all_matches,
const std::vector<cv::DMatch>& inlier_matches,
unsigned int marker_id,
bool draw_outlier) const {
std::clock_t starttime=std::clock();
if (ransac_marker_pub_.getNumSubscribers() > 0) { //don't visualize, if nobody's looking
visualization_msgs::Marker marker_lines;
marker_lines.header.frame_id = "/openni_rgb_optical_frame";
marker_lines.ns = "ransac_markers";
marker_lines.header.stamp = ros::Time::now();
marker_lines.action = visualization_msgs::Marker::ADD;
marker_lines.pose.orientation.w = 1.0;
marker_lines.id = marker_id;
marker_lines.type = visualization_msgs::Marker::LINE_LIST;
marker_lines.scale.x = 0.002;
std_msgs::ColorRGBA color_red ; //red outlier
color_red.r = 1.0;
color_red.a = 1.0;
std_msgs::ColorRGBA color_green; //green inlier, newer endpoint
color_green.g = 1.0;
color_green.a = 1.0;
std_msgs::ColorRGBA color_yellow; //yellow inlier, earlier endpoint
color_yellow.r = 1.0;
color_yellow.g = 1.0;
color_yellow.a = 1.0;
std_msgs::ColorRGBA color_blue ; //red-blue outlier
color_blue.b = 1.0;
color_blue.a = 1.0;
marker_lines.color = color_green; //just to set the alpha channel to non-zero
AISNavigation::PoseGraph3D::Vertex* earlier_v; //used to get the transform
AISNavigation::PoseGraph3D::Vertex* newer_v; //used to get the transform
AISNavigation::PoseGraph3D::VertexIDMap v_idmap = optimizer_->vertices();
// end of initialization
ROS_DEBUG("Matches Visualization start");
// write all inital matches to the line_list
marker_lines.points.clear();//necessary?
if (draw_outlier)
{
for (unsigned int i=0; i<all_matches.size(); i++) {
int newer_id = all_matches.at(i).queryIdx; //feature id in newer node
int earlier_id = all_matches.at(i).trainIdx; //feature id in earlier node
earlier_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[earlier_node.id_]);
newer_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[newer_node.id_]);
//Outliers are red (newer) to blue (older)
marker_lines.colors.push_back(color_red);
marker_lines.colors.push_back(color_blue);
marker_lines.points.push_back(
pointInWorldFrame(newer_node.feature_locations_3d_[newer_id],
newer_v->transformation));
marker_lines.points.push_back(
pointInWorldFrame(earlier_node.feature_locations_3d_[earlier_id],
earlier_v->transformation));
}
}
for (unsigned int i=0; i<inlier_matches.size(); i++) {
int newer_id = inlier_matches.at(i).queryIdx; //feature id in newer node
int earlier_id = inlier_matches.at(i).trainIdx; //feature id in earlier node
earlier_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[earlier_node.id_]);
newer_v = static_cast<AISNavigation::PoseGraph3D::Vertex*>(v_idmap[newer_node.id_]);
//inliers are green (newer) to blue (older)
marker_lines.colors.push_back(color_green);
marker_lines.colors.push_back(color_blue);
marker_lines.points.push_back(
pointInWorldFrame(newer_node.feature_locations_3d_[newer_id],
newer_v->transformation));
marker_lines.points.push_back(
pointInWorldFrame(earlier_node.feature_locations_3d_[earlier_id],
earlier_v->transformation));
}
ransac_marker_pub_.publish(marker_lines);
ROS_DEBUG_STREAM("Published " << marker_lines.points.size()/2 << " lines");
}
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");
}
void JointPositionController::update(const ros::Time &time, const ros::Duration &period) {
ROS_DEBUG_STREAM("updating");
// compute velocities and accelerations by hand just to be sure
for (int i = 0; i < n_joints_; i++) {
double current_position = joints_[i].getPosition();
current_velocities_[i] = (current_position - previous_positions_[i]) / period.toSec();
current_accelerations_[i] = (current_velocities_[i] - previous_velocities_[i]) / period.toSec();
previous_positions_[i] = current_position;
previous_velocities_[i] = current_velocities_[i];
}
if (new_reference_) {
// Read the latest commanded trajectory message
commanded_trajectory_ = *trajectory_command_buffer_.readFromRT();
new_reference_ = false;
for (int i = 0; i < n_joints_; i ++) {
if (std::abs(commanded_trajectory_.positions[i] - last_commanded_trajectory_.positions[i]) > command_update_tolerance_) {
must_recompute_trajectory_ = true;
reached_reference_ = false;
last_commanded_trajectory_ = commanded_trajectory_;
}
}
}
// Initialize RML result
int rml_result = 0;
ROS_DEBUG_STREAM("checking for having to recompute");
// Compute RML traj after the start time and if there are still points in the queue
if (must_recompute_trajectory_) {
// Compute the trajectory
ROS_WARN_STREAM("RML Recomputing trajectory...");
// Update RML input parameters
for (int i = 0; i < n_joints_; i++) {
rml_in_->CurrentPositionVector->VecData[i] = joints_[i].getPosition();
rml_in_->CurrentVelocityVector->VecData[i] = current_velocities_[i];
rml_in_->CurrentAccelerationVector->VecData[i] = current_accelerations_[i];
rml_in_->TargetPositionVector->VecData[i] = commanded_trajectory_.positions[i];
rml_in_->TargetVelocityVector->VecData[i] = commanded_trajectory_.velocities[i];
rml_in_->SelectionVector->VecData[i] = true;
}
ROS_DEBUG_STREAM("Current position: " << std::endl << *(rml_in_->CurrentPositionVector));
ROS_DEBUG_STREAM("Target position: " << std::endl << *(rml_in_->TargetPositionVector));
// Store the traj start time
traj_start_time_ = time;
// Set desired execution time for this trajectory (definitely > 0)
rml_in_->SetMinimumSynchronizationTime(minimum_synchronization_time_);
// ROS_DEBUG_STREAM("RML IN: time: " << rml_in_->GetMinimumSynchronizationTime());
// Specify behavior after reaching point
rml_flags_.BehaviorAfterFinalStateOfMotionIsReached = recompute_trajectory_ ? RMLPositionFlags::RECOMPUTE_TRAJECTORY : RMLPositionFlags::KEEP_TARGET_VELOCITY;
rml_flags_.SynchronizationBehavior = RMLPositionFlags::ONLY_TIME_SYNCHRONIZATION;
rml_flags_.KeepCurrentVelocityInCaseOfFallbackStrategy = true;
// Compute trajectory
rml_result = rml_->RMLPosition(*rml_in_.get(),
rml_out_.get(),
rml_flags_);
// Disable recompute flag
must_recompute_trajectory_ = false;
}
// Sample the already computed trajectory
rml_result = rml_->RMLPositionAtAGivenSampleTime(
(time - traj_start_time_ + period).toSec(),
rml_out_.get());
// Determine if any of the joint tolerances have been violated
for (int i = 0; i < n_joints_; i++) {
double tracking_error = std::abs(rml_out_->NewPositionVector->VecData[i] - joints_[i].getPosition());
if (tracking_error > position_tolerances_[i]) {
must_recompute_trajectory_ = true;
ROS_WARN_STREAM("Tracking for joint " << i << " outside of tolerance! (" << tracking_error
<< " > " << position_tolerances_[i] << ")");
}
}
// Compute command
for (int i = 0; i < n_joints_; i++) {
commanded_positions_[i] = rml_out_->NewPositionVector->VecData[i];
}
// Only set a different position command if the
switch (rml_result) {
case ReflexxesAPI::RML_WORKING:
// S'all good.
break;
case ReflexxesAPI::RML_FINAL_STATE_REACHED:
ROS_DEBUG_STREAM("final state reached");
must_recompute_trajectory_ = recompute_trajectory_;
reached_reference_ = true;
break;
default:
if (loop_count_ % decimation_ == 0) {
ROS_ERROR("Reflexxes error code: %d. Setting position commands to measured position.", rml_result);
}
for (int i = 0; i < n_joints_; i++)
commanded_positions_[i] = joints_[i].getPosition();
break;
};
// Set the lower-level commands
if (!reached_reference_) {
ROS_DEBUG_STREAM("setting command");
for (int i = 0; i < n_joints_; i++) {
joints_[i].setCommand(commanded_positions_[i]);
}
}
// Publish state
if (loop_count_ % decimation_ == 0) {
/*
* boost::scoped_ptr<realtime_tools::RealtimePublisher<controllers_msgs::JointControllerState> >
* &state_pub = controller_state_publisher_;
*
* for(int i=0; i<n_joints_; i++) {
* if(state_pub && state_pub->trylock()) {
* state_pub->msg_.header.stamp = time;
* state_pub->msg_.set_point = pos_target;
* state_pub->msg_.process_value = pos_actual;
* state_pub->msg_.process_value_dot = vel_actual;
* state_pub->msg_.error = pos_error;
* state_pub->msg_.time_step = period.toSec();
* state_pub->msg_.command = commanded_effort;
*
* double dummy;
* pids_[i]->getGains(
* state_pub->msg_.p,
* state_pub->msg_.i,
* state_pub->msg_.d,
* state_pub->msg_.i_clamp,
* dummy);
* state_pub->unlockAndPublish();
}
}
*/
}
// Increment the loop count
loop_count_++;
}
/** Open the camera device.
*
* @param newconfig configuration parameters
* @return true, if successful
*
* @post diagnostics frequency parameters set
*
* if successful:
* state_ is Driver::OPENED
* camera_name_ set to GUID string
* GUID configuration parameter updated
*/
bool NaoqiCameraDriver::openCamera(Config &newconfig)
{
bool success = false;
try
{
camera_proxy_ = boost::shared_ptr<ALVideoDeviceProxy>(new ALVideoDeviceProxy(m_broker));
if (camera_proxy_->getGenericProxy()->isLocal())
ROS_INFO("nao_camera runs directly on the robot.");
else
ROS_WARN("nao_camera runs remotely.");
// build a unique name for the camera that will be used to
// store calibration informations.
stringstream canonical_name;
canonical_name << "nao_camera"
<< "_src" << newconfig.source
<< "_res" << newconfig.resolution;
camera_name_ = camera_proxy_->subscribeCamera(
camera_name_,
newconfig.source,
newconfig.resolution,
kRGBColorSpace,
newconfig.frame_rate);
if (!cinfo_->setCameraName(canonical_name.str()))
{
// GUID is 16 hex digits, which should be valid.
// If not, use it for log messages anyway.
ROS_WARN_STREAM("[" << camera_name_
<< "] name not valid"
<< " for camera_info_manger");
}
ROS_INFO_STREAM("[" << camera_name_ << "] opened: resolution "
<< newconfig.resolution << " @"
<< newconfig.frame_rate << " fps");
state_ = Driver::OPENED;
calibration_matches_ = true;
newconfig.guid = camera_name_; // update configuration parameter
retries_ = 0;
success = true;
}
catch (const ALError& e)
{
state_ = Driver::CLOSED; // since the open() failed
if (retries_++ > 0)
ROS_DEBUG_STREAM("[" << camera_name_
<< "] exception opening device (retrying): "
<< e.what());
else
ROS_ERROR_STREAM("[" << camera_name_
<< "] device open failed: " << e.what());
}
// update diagnostics parameters
diagnostics_.setHardwareID(camera_name_);
double delta = newconfig.frame_rate * 0.1; // allow 10% error margin
topic_diagnostics_min_freq_ = newconfig.frame_rate - delta;
topic_diagnostics_max_freq_ = newconfig.frame_rate + delta;
return success;
}
/***********************************************************************
* Method: RobotController::Init
* Params: ros::NodeHandle *nh, int robotID, std::string robotName, int storage_cap, int storage_used, bool type
* Returns: void
* Effects: Initialize the controller with parameters
***********************************************************************/
void RobotController::Init(ros::NodeHandle *nh, int robotID, std::string robotName, int storage_cap, int storage_used, RobotState::Type type)
{
m_nh = nh;
m_listener.reset( new tf::TransformListener(*nh) );
if (m_nh->getParam("controller/robot_id", robotID))
ROS_INFO_STREAM("Set robot ID: " << robotID);
else
ROS_ERROR_STREAM("Did not read robot ID: default = " << robotID);
m_status.SetID(robotID);
if (m_nh->getParam("controller/robot_name", robotName))
ROS_INFO_STREAM("Read robot name: " << robotName);
else
ROS_ERROR_STREAM("Did not read robot name: default = " << robotName);
m_status.SetName(robotName);
if(m_nh->getParam("controller/storage_capacity", storage_cap))
ROS_INFO_STREAM("Read storage capacity: " << storage_cap);
else
ROS_ERROR_STREAM("Did not read storage capacity: default = " << storage_cap);
m_status.SetStorageCapacity(storage_cap);
if(m_nh->getParam("controller/storage_used", storage_used))
ROS_INFO_STREAM("Read storage used: " << storage_used);
else
ROS_ERROR_STREAM("Did not read storage used: default = " << storage_used);
m_status.SetStorageUsed(storage_used);
std::string robotType;
if (m_nh->getParam("controller/type", robotType))
ROS_INFO_STREAM("Read robot type: " << robotType);
else
ROS_ERROR_STREAM("Did not read type: default = " << robotType);
if (robotType.compare("collector") == 0)
{
ROS_INFO_STREAM("Type: Collector");
type = RobotState::COLLECTOR_BOT;
}
else
{
ROS_INFO_STREAM("Type: Binbot");
type = RobotState::BIN_BOT;
}
m_status.SetType(type);
std::string tf_prefix;
if (m_nh->getParam("controller/tf_prefix", tf_prefix))
ROS_INFO_STREAM("Read tf prefix: " << tf_prefix);
else
ROS_ERROR_STREAM("Did not read tf_prefix: default = " << tf_prefix);
base_frame = std::string("base_link");
if (m_nh->getParam("controller/base_frame", base_frame))
ROS_INFO_STREAM("Read base frame: " << base_frame);
else
ROS_ERROR_STREAM("Did not read base_frame: default = " << base_frame);
base_frame = tf_prefix + "/"+base_frame;
if (robotID < 0)
{
ROS_ERROR_STREAM("ERROR: Received invalid robot id: " << robotID);
return;
}
if (storage_cap < 0)
{
ROS_ERROR_STREAM("ERROR: invalid storage capacity: " << storage_cap);
return;
}
if (storage_used > storage_cap)
{
ROS_ERROR_STREAM("ERROR: storage used > storage capacity: used=" << storage_used << ", cap=" << storage_cap);
return;
}
// Todo: Only start if fake trash is not used
m_tagProcessor.reset( new AprilTagProcessor() );
m_tagProcessor->Init(nh, robotID);
action_client_ptr.reset( new MoveBaseClient("move_base", true) );
// Wait for the action server to come up
while(ros::ok() && !action_client_ptr->waitForServer(ros::Duration(2.0))){
ROS_INFO_THROTTLE(3.0,"Waiting for the move_base action server to come up");
}
SetupCallbacks();
ROS_DEBUG_STREAM("Robot has setup the movebase client");
Transition(RobotState::WAITING);
m_timeEnteringState = ros::Time::now();
ROS_INFO_STREAM("Finished initializing");
}
// ******************************************************************************************
// Save package using default path
// ******************************************************************************************
bool ConfigurationFilesWidget::generatePackage()
{
// Get path name
std::string new_package_path = stack_path_->getPath();
// Check that a valid stack package name has been given
if( new_package_path.empty() )
{
QMessageBox::warning( this, "Error Generating", "No package path provided. Please choose a directory location to generate the MoveIt configuration files." );
return false;
}
// Check setup assist deps
if( !checkDependencies() )
return false; // canceled
// Check that all groups have components
if( !noGroupsEmpty() )
return false; // not ready
// Trim whitespace from user input
boost::trim( new_package_path );
// Get the package name ---------------------------------------------------------------------------------
new_package_name_ = getPackageName( new_package_path );
const std::string setup_assistant_file = config_data_->appendPaths( new_package_path, ".setup_assistant" );
// Make sure old package is correct package type and verify over write
if( fs::is_directory( new_package_path ) && !fs::is_empty( new_package_path ) )
{
// Check if the old package is a setup assistant package. If it is not, quit
if( ! fs::is_regular_file( setup_assistant_file ) )
{
QMessageBox::warning( this, "Incorrect Folder/Package",
QString("The chosen package location already exists but was not previously created using this MoveIt Setup Assistant. If this is a mistake, replace the missing file: ")
.append( setup_assistant_file.c_str() ) );
return false;
}
// Confirm overwrite
if( QMessageBox::question( this, "Confirm Package Update",
QString("Are you sure you want to overwrite this existing package with updated configurations?<br /><i>")
.append( new_package_path.c_str() )
.append( "</i>" ),
QMessageBox::Ok | QMessageBox::Cancel)
== QMessageBox::Cancel )
{
return false; // abort
}
}
else // this is a new package (but maybe the folder already exists)
{
// Create new directory ------------------------------------------------------------------
try
{
fs::create_directory( new_package_path ) && !fs::is_directory( new_package_path );
}
catch( ... )
{
QMessageBox::critical( this, "Error Generating Files",
QString("Unable to create directory ").append( new_package_path.c_str() ) );
return false;
}
}
// Begin to create files and folders ----------------------------------------------------------------------
std::string absolute_path;
for (int i = 0; i < gen_files_.size(); ++i)
{
GenerateFile* file = &gen_files_[i];
// Check if we should skip this file
if( !file->generate_ )
{
continue;
}
// Create the absolute path
absolute_path = config_data_->appendPaths( new_package_path, file->rel_path_ );
ROS_DEBUG_STREAM("Creating file " << absolute_path );
// Run the generate function
if( !file->gen_func_(absolute_path) )
{
// Error occured
QMessageBox::critical( this, "Error Generating File",
QString("Failed to generate folder or file: '")
.append( file->rel_path_.c_str() ).append("' at location:\n").append( absolute_path.c_str() ));
return false;
}
updateProgress(); // Increment and update GUI
}
return true;
}
bool planning_environment::removeCompletedTrajectory(const boost::shared_ptr<urdf::Model> &model,
const trajectory_msgs::JointTrajectory &trajectory_in,
const sensor_msgs::JointState& current_state,
trajectory_msgs::JointTrajectory &trajectory_out,
bool zero_vel_acc)
{
trajectory_out = trajectory_in;
trajectory_out.points.clear();
if(trajectory_in.points.empty())
{
ROS_WARN("No points in input trajectory");
return true;
}
int current_position_index = 0;
//Get closest state in given trajectory
current_position_index = closestStateOnTrajectory(model,
trajectory_in,
current_state,
current_position_index,
trajectory_in.points.size() - 1);
if (current_position_index < 0)
{
ROS_ERROR("Unable to identify current state in trajectory");
return false;
} else {
ROS_DEBUG_STREAM("Closest state is " << current_position_index << " of " << trajectory_in.points.size());
}
// Start one ahead of returned closest state index to make sure first trajectory point is not behind current state
for(unsigned int i = current_position_index+1; i < trajectory_in.points.size(); ++i)
{
trajectory_out.points.push_back(trajectory_in.points[i]);
}
if(trajectory_out.points.empty())
{
ROS_DEBUG("No points in output trajectory");
return false;
}
ros::Duration first_time = trajectory_out.points[0].time_from_start;
if(first_time < ros::Duration(.1)) {
first_time = ros::Duration(0.0);
} else {
first_time -= ros::Duration(.1);
}
for(unsigned int i=0; i < trajectory_out.points.size(); ++i)
{
if(trajectory_out.points[i].time_from_start > first_time) {
trajectory_out.points[i].time_from_start -= first_time;
} else {
ROS_INFO_STREAM("Not enough time in time from start for trajectory point " << i);
}
}
if(zero_vel_acc) {
for(unsigned int i=0; i < trajectory_out.joint_names.size(); ++i) {
for(unsigned int j=0; j < trajectory_out.points.size(); ++j) {
trajectory_out.points[j].velocities[i] = 0;
trajectory_out.points[j].accelerations[i] = 0;
}
}
}
return true;
}
/** Get feature values.
*
* @pre feature_set_ initialized for this camera
*
* @param finfo pointer to information for this feature
* @param value [out] pointer where parameter value stored
* @param value2 [out] optional pointer for second parameter value
* for white balance. Otherwise NULL.
*/
void Features::getValues(dc1394feature_info_t *finfo,
double *value, double *value2)
{
dc1394feature_t feature = finfo->id;
dc1394error_t rc;
if (!finfo->readout_capable)
{
ROS_INFO_STREAM("feature " << featureName(feature)
<< " value not available from device");
return;
}
if (feature == DC1394_FEATURE_WHITE_BALANCE)
{
// handle White Balance separately, it has two components
if (finfo->absolute_capable && finfo->abs_control)
{
// supports reading and setting float value
// @todo get absolute White Balance values
rc = DC1394_FUNCTION_NOT_SUPPORTED;
}
else
{
// get integer White Balance values
uint32_t bu_val;
uint32_t rv_val;
rc = dc1394_feature_whitebalance_get_value(camera_, &bu_val, &rv_val);
if (DC1394_SUCCESS == rc)
{
// convert to double
*value = bu_val;
*value2 = rv_val;
}
}
if (DC1394_SUCCESS == rc)
{
ROS_DEBUG_STREAM("feature " << featureName(feature)
<< " Blue/U: " << *value
<< " Red/V: " << *value2);
}
else
{
ROS_WARN_STREAM("failed to get values for feature "
<< featureName(feature));
}
}
else
{
// other features only have one component
if (finfo->absolute_capable && finfo->abs_control)
{
// supports reading and setting float value
float fval;
rc = dc1394_feature_get_absolute_value(camera_, feature, &fval);
if (DC1394_SUCCESS == rc)
{
*value = fval; // convert to double
}
}
else // no float representation
{
uint32_t ival;
rc = dc1394_feature_get_value(camera_, feature, &ival);
if (DC1394_SUCCESS == rc)
{
*value = ival; // convert to double
}
}
if (DC1394_SUCCESS == rc)
{
ROS_DEBUG_STREAM("feature " << featureName(feature)
<< " has value " << *value);
}
else
{
ROS_WARN_STREAM("failed to get value of feature "
<< featureName(feature));
}
}
}
void waypoints_follower::run()
{
const double MAX_TWIST_LINEAR = 1.0;
const double MAX_TWIST_ANGULAR = 0.5;
const double TURNING_RADIUS=0.4;
if (!active) return;
// ROS_INFO("active");
if (!localized)
{
ROS_WARN("waiting for localization");
return;
}
if (reached(next_target) || start_new_target) //Change of target!
{
start_new_target = false;
if (targets.size()==0)
{
deactivate(deactivate_reason::NO_MORE_TARGETS);
return;
}
std::unique_lock<std::mutex>(targets_mtx);
next_target = targets.front();
targets.pop_front();
geometry_msgs::Pose2D current_pose;
current_pose.x=x;
current_pose.y=y;
xtarget = next_target.x;
ytarget = next_target.y;
ROS_INFO_STREAM("starting new target "<<xtarget<< " " <<ytarget<<" "<<int(next_target.z)%1000);
straight=true;
turning=false;
}
/* More complex implementation, for the future, it chooses speed and angular radius
if (distance(next_target)<TURNING_RADIUS && !turning && targets.size()>0 && !start_new_target) //Will not use circle if this is the last target or if we are starting now
{
//TODO start turning along a circle
//Find next heading, circle radius, circle length, circle angle
double current_heading=theta;
double heading=atan2(ytarget-targets.front().y,xtarget-targets.front().x);
double current_speed=twist.linear.x;
double next_speed=distance(next_target,targets.front())/(targets.front().z-next_target.z);
double delta_heading=heading-current_heading;
bool rotate_left;
if((0<delta_heading && delta_heading<M_PI) || (-2*M_PI<delta_heading && delta_heading<-M_PI ))
rotate_left=true;
else rotate_left=false;
bool ok=false;
double desired_speed=(current_speed+next_speed)/2.0;
double radius=max_turning_radius/10.0;
double alpha=2*radius/wheel_distance;
while(!ok)
{
double right_wheel_speed=-desired_speed*(1/alpha+1);
if (right_wheel_speed>max_speed) //Too fast, slow down
{
right_wheel_speed=max_speed;
}
double left_wheel_speed=2*desired_speed-right_wheel_speed;
if (left_wheel_speed>max_speed) //Also too fast, change speed and turning radius
{
desired_speed=0.9*desired_speed;
radius=radius+max_turning_radius*0.1;
}
else
{
ok=true;
}
}
turning = true;
twist.linear.x=desired_speed;
twist.angular.z=(right_wheel_speed-left_wheel_speed)/2.0;
}
*/
if (distance(next_target)<TURNING_RADIUS && distance(next_target)>reached_threshold && !turning && targets.size()>0 && !start_new_target) //Will not use circle if this is the last target or if we are starting now
{
turning=true;
straight=false;
desired_heading=atan2(targets.front().y-ytarget,targets.front().x-xtarget);
double current_speed=twist.linear.x;
double next_speed=distance(next_target,targets.front())/(targets.front().z-next_target.z);
if(desired_speed>MAX_TWIST_LINEAR) desired_speed=MAX_TWIST_LINEAR;
desired_speed=(current_speed+next_speed)/2.0;
twist.linear.x=desired_speed;
ROS_INFO_STREAM("starting turning from near "<<xtarget<< " " <<ytarget<<" "<<int(next_target.z)%1000<< "to next target");
}
double length = distance(next_target);
double theta_err;
if (turning)
{
//either keep publishing same rotation and speed or feedback on the circle information
twist.linear.x=desired_speed*(1+ki1*(TURNING_RADIUS-length));
twist.angular.z=-ki2*sin(desired_heading-theta);
theta_err=desired_heading-theta;
if (fabs(desired_heading-theta)<0.2 )//|| distance())
{
//We kind of reached the desired heading, we should switch to the next target and stop turning
std::unique_lock<std::mutex>(targets_mtx);
next_target = targets.front();
targets.pop_front();
geometry_msgs::Pose2D current_pose;
current_pose.x=x;
current_pose.y=y;
xtarget = next_target.x;
ytarget = next_target.y;
straight=true;
ROS_INFO_STREAM("starting new target "<<xtarget<< " " <<ytarget<<" "<<int(next_target.z)%1000);
ROS_INFO("starting straight");
turning=false;
}
}
if (straight)
{
double delta = next_target.z - ros::Time::now().toSec();
if (delta<-0)
{
twist.linear.x = 0;
ROS_WARN_STREAM("Target time is in the past, stopping");
deactivate(deactivate_reason::GLOBAL_TARGET_NOT_REACHED);
}
else
{
twist.linear.x=length/delta*1.1;
theta_err=atan2(ytarget-y,xtarget-x)-theta;
if (fabs(fabs(theta_err)-M_PI)<0.1) theta_err=theta_err+0.1;
twist.angular.z=-kp2*sin(theta_err);
}
}
twist.linear.x = std::min(twist.linear.x,MAX_TWIST_LINEAR);
comand_pub.publish(twist);
ROS_DEBUG_STREAM("controller run xt: "<<xtarget<<" yt: "<<ytarget<<" x: "<<x<<" y: "<<y<<" t "<<next_target.z);
ROS_DEBUG_STREAM("controller run theta:"<<theta<<" error "<<theta_err<<" v: "<<twist.linear.x<<" w: "<<twist.angular.z);
ros::spinOnce();
}
/** Configure a feature for the currently open device.
*
* @pre feature_set_ initialized
*
* @param feature desired feature number
* @param control [in,out] pointer to control parameter (may change)
* @param value [in,out] pointer to requested parameter value (may
* change depending on device restrictions)
* @param value2 [in,out] optional pointer to second parameter value
* for white balance (may change depending on device
* restrictions). No second value if NULL pointer.
*
* The parameter values are represented as double despite the fact
* that most features on most cameras only allow unsigned 12-bit
* values. The exception is the rare feature that supports
* "absolute" values in 32-bit IEEE float format. Double can
* represent all possible option values accurately.
*/
void Features::configure(dc1394feature_t feature, int *control,
double *value, double *value2)
{
// device-relevant information for this feature
dc1394feature_info_t *finfo =
&feature_set_.feature[feature - DC1394_FEATURE_MIN];
if (!finfo->available) // feature not available?
{
*control = camera1394::Camera1394_None;
return;
}
switch (*control)
{
case camera1394::Camera1394_Off:
setOff(finfo);
break;
case camera1394::Camera1394_Query:
getValues(finfo, value, value2);
break;
case camera1394::Camera1394_Auto:
if (!setMode(finfo, DC1394_FEATURE_MODE_AUTO))
{
setOff(finfo);
}
break;
case camera1394::Camera1394_Manual:
if (!setMode(finfo, DC1394_FEATURE_MODE_MANUAL))
{
setOff(finfo);
break;
}
// TODO: break this into some internal methods
if (finfo->absolute_capable && finfo->abs_control)
{
// supports reading and setting float value
float fmin, fmax;
if (DC1394_SUCCESS ==
dc1394_feature_get_absolute_boundaries(camera_, feature,
&fmin, &fmax))
{
// clamp *value between minimum and maximum
if (*value < fmin)
*value = (double) fmin;
else if (*value > fmax)
*value = (double) fmax;
}
else
{
ROS_WARN_STREAM("failed to get feature "
<< featureName(feature) << " boundaries ");
}
// @todo handle absolute White Balance values
float fval = *value;
if (DC1394_SUCCESS !=
dc1394_feature_set_absolute_value(camera_, feature, fval))
{
ROS_WARN_STREAM("failed to set feature "
<< featureName(feature) << " to " << fval);
}
}
else // no float representation
{
// round requested value to nearest integer
*value = rint(*value);
// clamp *value between minimum and maximum
if (*value < finfo->min)
*value = (double) finfo->min;
else if (*value > finfo->max)
*value = (double) finfo->max;
dc1394error_t rc;
uint32_t ival = (uint32_t) *value;
// handle White Balance, which has two components
if (feature == DC1394_FEATURE_WHITE_BALANCE)
{
*value2 = rint(*value2);
// clamp *value2 between same minimum and maximum
if (*value2 < finfo->min)
*value2 = (double) finfo->min;
else if (*value2 > finfo->max)
*value2 = (double) finfo->max;
uint32_t ival2 = (uint32_t) *value2;
rc = dc1394_feature_whitebalance_set_value(camera_, ival, ival2);
}
else
{
// other features only have one component
rc = dc1394_feature_set_value(camera_, feature, ival);
}
if (rc != DC1394_SUCCESS)
{
ROS_WARN_STREAM("failed to set feature "
<< featureName(feature) << " to " << ival);
}
}
break;
case camera1394::Camera1394_OnePush:
// Try to set OnePush mode
setMode(finfo, DC1394_FEATURE_MODE_ONE_PUSH_AUTO);
// Now turn the control off, so camera does not continue adjusting
setOff(finfo);
break;
case camera1394::Camera1394_None:
// Invalid user input, because this feature actually does exist.
ROS_INFO_STREAM("feature " << featureName(feature)
<< " exists, cannot set to None");
break;
default:
ROS_WARN_STREAM("unknown state (" << *control
<<") for feature " << featureName(feature));
}
// return actual state reported by the device
*control = getState(finfo);
ROS_DEBUG_STREAM("feature " << featureName(feature)
<< " now in state " << *control);
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboTruck::Update()
{
boost::mutex::scoped_lock scoped_lock(lock);
if ( terminated_ )
{
return;
}
// std::cerr << link_->GetWorldPose() << std::endl;
common::Time current_time = world_->GetSimTime();
double delta_time = (current_time-last_time_).Double();
double theta = acos(CIRCLE_RADIUS/(CIRCLE_DISTANCE/2));
double x, y, yaw;
double l1 = CIRCLE_DISTANCE*sin(theta);
double l2 = 2*CIRCLE_RADIUS*(M_PI-theta);
double all_l = l1 + l2 + l1 + l2;
// static const float VELOCITY = 4.16667; // 4.16667 m/sec = 15 km/h, 2.77778 m/sec = 10 km/h, 1.38889 = 5 km/h
if ( current_time.Double() < 6*60 )
{
traversed_ += 4.16667 * delta_time;
}
else if ( current_time.Double() < 12*60 )
{
traversed_ += 2.77778 * delta_time;
}
else if ( current_time.Double() < 20*60 )
{
traversed_ += 1.38889 * delta_time;
}
else
{
ROS_FATAL("Time's up, Your challenge was over");
terminated_ = true;
}
double l = fmod(traversed_, all_l);
ROS_DEBUG_STREAM("time: " << current_time.Double() << ", traversed: " << traversed_);
if ( l < l1 )
{
x = (l - l1/2)*sin(theta);
y = -(l - l1/2)*cos(theta);
yaw = theta-M_PI/2;
}
else if ( l < l1 + l2 )
{
l = l - l1;
x = -CIRCLE_RADIUS * cos(l/l2*(2*M_PI-theta*2)+theta) + CIRCLE_DISTANCE/2;
y = -CIRCLE_RADIUS * sin(l/l2*(2*M_PI-theta*2)+theta);
yaw = (l/l2*(2*M_PI-theta*2)+theta)-M_PI/2;
}
else if ( l < l1 + l2 + l1 )
{
l = l - (l1 + l2);
x = -(l - l1/2)*sin(theta);
y = -(l - l1/2)*cos(theta);
yaw =-M_PI/2-theta;
}
else
{
l = l - (l1 + l2 + l1);
x = CIRCLE_RADIUS * cos(l/l2*(2*M_PI-theta*2)+theta) - CIRCLE_DISTANCE/2;
y = -CIRCLE_RADIUS * sin(l/l2*(2*M_PI-theta*2)+theta);
yaw = -(l/l2*(2*M_PI-theta*2)+theta)-M_PI/2;
}
model_->SetLinkWorldPose(math::Pose(x, y, 0.595, 0, 0, yaw), link_);
last_time_ = world_->GetSimTime();
// check score
// void Entity::GetNearestEntityBelow(double &_distBelow, std::string &_entityName)
gazebo::physics::RayShapePtr rayShape = boost::dynamic_pointer_cast<gazebo::physics::RayShape>(
world_->GetPhysicsEngine()->CreateShape("ray", gazebo::physics::CollisionPtr()));
double distAbove;
std::string entityName;
math::Box box = model_->GetLink("heliport")->GetCollisionBoundingBox();
math::Vector3 start = model_->GetLink("heliport")->GetWorldPose().pos;
math::Vector3 end = start;
start.z = box.max.z + 0.00001;
end.z += 1000;
rayShape->SetPoints(start, end);
rayShape->GetIntersection(distAbove, entityName);
distAbove -= 0.00001;
// publish remain time
std::stringstream ss;
std_msgs::String msg_time;
ss << 20*60 - current_time.Double();
msg_time.data = "remain time:" + ss.str();
pub_time_.publish(msg_time);
if ( entityName != "" && distAbove < 1.0 )
{
std_msgs::String msg_score, msg_time;
msg_score.data = "Mission Completed";
ROS_INFO_STREAM("Remaining time is " << msg_time.data << "[sec], Score is " << msg_score.data);
pub_score_.publish(msg_score);
terminated_ = true;
}
}
// General outline: the model can be written:
// M(o)o_dotdot + Q(o,o_dot)o_dot + G(o) = tau
// When we linearize to the first order around position O, we get the approximation: o = O + e and
// M(O)e_dotdot + Q(O,O_dot)e_dot + G(O) + grad(G)(O)e = tau
// Or:
// (e)_dot = I e_dot + 0
// (e_dot)_dot = -M^-1 Q e_dot -M^-1 G e + M^-1 tau - G(O)
// We first compute the matrices M,Q,G and then linearize the gravity term.
bool SerialChainModel::getLinearization(const StateVector & x, StateMatrix & A, InputMatrix & B, InputVector & c)
{
// ROS_DEBUG("1");
assert(inputs_>0);
const int n=inputs_;
KDL::JntArray kdl_q(n);
KDL::JntArray kdl_q_dot(n);
// ROS_DEBUG("2");
//Fills the data
for(int i=0; i<n; ++i)
{
kdl_q(i) = x(i);
kdl_q_dot(i) = x(i+n);
}
// ROS_DEBUG("3");
//Compute M
Eigen::MatrixXd M(n,n);
NEWMAT::Matrix mass(n,n);
chain_->computeMassMatrix(kdl_q,kdl_torque_,mass);
for(int i=0;i<n;i++)
for(int j=0;j<n;j++)
M(i,j)=mass(i+1,j+1);
// ROS_DEBUG("4");
//Compute Q
Eigen::MatrixXd Q(n,n);
Q.setZero();
NEWMAT::Matrix christoffel(n*n,n);
chain_->computeChristoffelSymbols(kdl_q,kdl_torque_,christoffel);
// ROS_DEBUG("5-");
for(int i=0;i<n;i++)
for(int j=0;j<n;j++)
for(int k=0;k<n;k++)
Q(i,j)+=christoffel(i*n+j+1,k+1)*kdl_q_dot(j);
// ROS_DEBUG("5");
//Compute G
Eigen::VectorXd G(n,1);
chain_->computeGravityTerms(kdl_q,kdl_torque_);
for(int i=0;i<n;i++)
G(i)=kdl_torque_[i][2];
// ROS_DEBUG("6");
//Computing the gradient of G
Eigen::MatrixXd gG(n,n);
const double epsi=0.01;
for(int i=0;i<n;i++)
{
KDL::JntArray ja = kdl_q;
ja(i)=ja(i)+epsi;
chain_->computeGravityTerms(ja,kdl_torque_);
for(int j=0;j<n;j++)
gG(i,j)=(kdl_torque_[j][2]-G(j))/epsi;
}
// ROS_DEBUG("7");
// Final assembling
Eigen::MatrixXd Minvminus=-M.inverse(); //Computing M's inverse once
ROS_DEBUG_STREAM(A.rows());
assert(A.rows()==n*2);
assert(A.cols()==n*2);
A.block(0,0,n,n).setZero();
A.block(0,n,n,n)=Eigen::MatrixXd::Identity(n,n);
A.block(n,0,n,n)=Minvminus*gG;
A.block(n,n,n,n)=Minvminus*Q;
assert(B.rows()==n*2);
assert(B.cols()==n);
B.block(0,0,n,n).setZero();
B.block(n,0,n,n)=Minvminus;
// ROS_DEBUG("8");
c=-G;
return true;
}
/**
* The callback function that is executed every time an image is received. It runs the main logic of the program.
*
* \param image_msg the ROS message containing the image to be processed
*/
void MPENode::imageCallback(const sensor_msgs::Image::ConstPtr& image_msg)
{
// Check whether already received the camera calibration data
if (!have_camera_info_)
{
ROS_WARN("No camera info yet...");
return;
}
// Import the image from ROS message to OpenCV mat
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat image = cv_ptr->image;
// Get time at which the image was taken. This time is used to stamp the estimated pose and also calculate the position of where to search for the makers in the image
double time_to_predict = image_msg->header.stamp.toSec();
const bool found_body_pose = trackable_object_.estimateBodyPose(image, time_to_predict);
if (found_body_pose) // Only output the pose, if the pose was updated (i.e. a valid pose was found).
{
//Eigen::Matrix4d transform = trackable_object.getPredictedPose();
Matrix6d cov = trackable_object_.getPoseCovariance();
Eigen::Matrix4d transform = trackable_object_.getPredictedPose();
ROS_DEBUG_STREAM("The transform: \n" << transform);
ROS_DEBUG_STREAM("The covariance: \n" << cov);
// Convert transform to PoseWithCovarianceStamped message
predicted_pose_.header.stamp = image_msg->header.stamp;
predicted_pose_.pose.pose.position.x = transform(0, 3);
predicted_pose_.pose.pose.position.y = transform(1, 3);
predicted_pose_.pose.pose.position.z = transform(2, 3);
Eigen::Quaterniond orientation = Eigen::Quaterniond(transform.block<3, 3>(0, 0));
predicted_pose_.pose.pose.orientation.x = orientation.x();
predicted_pose_.pose.pose.orientation.y = orientation.y();
predicted_pose_.pose.pose.orientation.z = orientation.z();
predicted_pose_.pose.pose.orientation.w = orientation.w();
// Add covariance to PoseWithCovarianceStamped message
for (unsigned i = 0; i < 6; ++i)
{
for (unsigned j = 0; j < 6; ++j)
{
predicted_pose_.pose.covariance.elems[j + 6 * i] = cov(i, j);
}
}
// Publish the pose
pose_pub_.publish(predicted_pose_);
}
else
{ // If pose was not updated
ROS_WARN("Unable to resolve a pose.");
}
// publish visualization image
if (image_pub_.getNumSubscribers() > 0)
{
cv::Mat visualized_image = image.clone();
cv::cvtColor(visualized_image, visualized_image, CV_GRAY2RGB);
if (found_body_pose)
{
trackable_object_.augmentImage(visualized_image);
}
// Publish image for visualization
cv_bridge::CvImage visualized_image_msg;
visualized_image_msg.header = image_msg->header;
visualized_image_msg.encoding = sensor_msgs::image_encodings::BGR8;
visualized_image_msg.image = visualized_image;
image_pub_.publish(visualized_image_msg.toImageMsg());
}
}
void ChompOptimizer::optimize()
{
ros::WallTime start_time = ros::WallTime::now();
double averageCostVelocity = 0.0;
int currentCostIter = 0;
int costWindow = 10;
std::vector<double>costs(costWindow, 0.0);
double minimaThreshold = 0.05;
bool should_break_out = false;
// if(parameters_->getAnimatePath())
// {
// animatePath();
// }
// iterate
for(iteration_ = 0; iteration_ < parameters_->getMaxIterations(); iteration_++)
{
ros::WallTime for_time = ros::WallTime::now();
performForwardKinematics();
//ROS_INFO_STREAM("Forward kinematics took " << (ros::WallTime::now()-for_time));
double cCost = getCollisionCost();
double sCost = getSmoothnessCost();
double cost = cCost + sCost;
//ROS_INFO_STREAM("Collision cost " << cCost << " smoothness cost " << sCost);
// if(parameters_->getAddRandomness() && currentCostIter != -1)
// {
// costs[currentCostIter] = cCost;
// currentCostIter++;
// if(currentCostIter >= costWindow)
// {
// for(int i = 1; i < costWindow; i++)
// {
// averageCostVelocity += (costs.at(i) - costs.at(i - 1));
// }
// averageCostVelocity /= (double)(costWindow);
// currentCostIter = -1;
// }
// }
if(iteration_ == 0)
{
best_group_trajectory_ = group_trajectory_.getTrajectory();
best_group_trajectory_cost_ = cost;
}
else
{
if(cost < best_group_trajectory_cost_)
{
best_group_trajectory_ = group_trajectory_.getTrajectory();
best_group_trajectory_cost_ = cost;
last_improvement_iteration_ = iteration_;
}
}
calculateSmoothnessIncrements();
ros::WallTime coll_time = ros::WallTime::now();
calculateCollisionIncrements();
ROS_DEBUG_STREAM("Collision increments took " << (ros::WallTime::now()-coll_time));
calculateTotalIncrements();
// if(!parameters_->getUseHamiltonianMonteCarlo())
// {
// // non-stochastic version:
addIncrementsToTrajectory();
// }
// else
// {
// // hamiltonian monte carlo updates:
// getRandomMomentum();
// updateMomentum();
// updatePositionFromMomentum();
// stochasticity_factor_ *= parameters_->getHmcAnnealingFactor();
// }
handleJointLimits();
updateFullTrajectory();
if(iteration_ % 10 == 0)
{
if(isCurrentTrajectoryMeshToMeshCollisionFree()) {
num_collision_free_iterations_ = 0;
ROS_INFO("Chomp Got mesh to mesh safety at iter %d. Breaking out early.", iteration_);
is_collision_free_ = true;
iteration_++;
should_break_out = true;
}
// } else if(safety == CollisionProximitySpace::InCollisionSafe) {
// ROS_DEBUG("Trajectory cost: %f (s=%f, c=%f)", getTrajectoryCost(), getSmoothnessCost(), getCollisionCost());
// CollisionProximitySpace::TrajectorySafety safety = checkCurrentIterValidity();
// if(safety == CollisionProximitySpace::MeshToMeshSafe)
// {
// num_collision_free_iterations_ = 0;
// ROS_INFO("Chomp Got mesh to mesh safety at iter %d. Breaking out early.", iteration_);
// is_collision_free_ = true;
// iteration_++;
// should_break_out = true;
// } else if(safety == CollisionProximitySpace::InCollisionSafe) {
// num_collision_free_iterations_ = parameters_->getMaxIterationsAfterCollisionFree();
// ROS_INFO("Chomp Got in collision safety at iter %d. Breaking out soon.", iteration_);
// is_collision_free_ = true;
// iteration_++;
// should_break_out = true;
// }
// else
// {
// is_collision_free_ = false;
// }
}
if(!parameters_->getFilterMode())
{
if(cCost < parameters_->getCollisionThreshold())
{
num_collision_free_iterations_ = parameters_->getMaxIterationsAfterCollisionFree();
is_collision_free_ = true;
iteration_++;
should_break_out = true;
} else {
//ROS_INFO_STREAM("cCost " << cCost << " over threshold " << parameters_->getCollisionThreshold());
}
}
if((ros::WallTime::now() - start_time).toSec() > parameters_->getPlanningTimeLimit() && !parameters_->getAnimatePath() && !parameters_->getAnimateEndeffector())
{
ROS_WARN("Breaking out early due to time limit constraints.");
break;
}
// if(fabs(averageCostVelocity) < minimaThreshold && currentCostIter == -1 && !is_collision_free_ && parameters_->getAddRandomness())
// {
// ROS_INFO("Detected local minima. Attempting to break out!");
// int iter = 0;
// bool success = false;
// while(iter < 20 && !success)
// {
// performForwardKinematics();
// double original_cost = getTrajectoryCost();
// group_trajectory_backup_ = group_trajectory_.getTrajectory();
// perturbTrajectory();
// handleJointLimits();
// updateFullTrajectory();
// performForwardKinematics();
// double new_cost = getTrajectoryCost();
// iter ++;
// if(new_cost < original_cost)
// {
// ROS_INFO("Got out of minimum in %d iters!", iter);
// averageCostVelocity = 0.0;
// currentCostIter = 0;
// success = true;
// }
// else
// {
// group_trajectory_.getTrajectory() = group_trajectory_backup_;
// updateFullTrajectory();
// currentCostIter = 0;
// averageCostVelocity = 0.0;
// success = false;
// }
// }
// if(!success)
// {
// ROS_INFO("Failed to exit minimum!");
// }
//}
else if(currentCostIter == -1)
{
currentCostIter = 0;
averageCostVelocity = 0.0;
}
// if(parameters_->getAnimateEndeffector())
// {
// animateEndeffector();
// }
// if(parameters_->getAnimatePath() && iteration_ % 25 == 0)
// {
// animatePath();
// }
if(should_break_out)
{
collision_free_iteration_++;
if(num_collision_free_iterations_ == 0) {
break;
} else if(collision_free_iteration_ > num_collision_free_iterations_)
{
// CollisionProximitySpace::TrajectorySafety safety = checkCurrentIterValidity();
// if(safety != CollisionProximitySpace::MeshToMeshSafe &&
// safety != CollisionProximitySpace::InCollisionSafe) {
// ROS_WARN_STREAM("Apparently regressed");
// }
break;
}
}
}
if(is_collision_free_)
{
ROS_INFO("Chomp path is collision free");
}
else
{
ROS_ERROR("Chomp path is not collision free!");
}
// if(parameters_->getAnimatePath())
// {
// animatePath();
// }
group_trajectory_.getTrajectory() = best_group_trajectory_;
updateFullTrajectory();
// if(parameters_->getAnimatePath())
// animatePath();
ROS_INFO("Terminated after %d iterations, using path from iteration %d", iteration_, last_improvement_iteration_);
ROS_INFO("Optimization core finished in %f sec", (ros::WallTime::now() - start_time).toSec() );
ROS_INFO_STREAM("Time per iteration " << (ros::WallTime::now() - start_time).toSec()/(iteration_*1.0));
}
void ImageProcessing::rawImageCallback(const sensor_msgs::Image::ConstPtr& image){
ros::Rate loop_rate(200);
double gray_level_precision;
double size_precision;
bool pause_at_each_frame = false; //Wait for user input each time a new frame is recieved.
ros::param::getCached(visp_camera_calibration::gray_level_precision_param,gray_level_precision);
ros::param::getCached(visp_camera_calibration::size_precision_param,size_precision);
ros::param::getCached(visp_camera_calibration::pause_at_each_frame_param,pause_at_each_frame);
vpPose pose;
vpCalibration calib;
visp_camera_calibration::CalibPointArray calib_all_points;
img_ = visp_bridge::toVispImage(*image);
init();
vpDisplay::display(img_);
vpDisplay::flush(img_);
if(!pause_at_each_frame){
vpImagePoint ip;
vpDisplay::displayRectangle(img_,0,0,img_.getWidth(),15,vpColor::black,true);
vpDisplay::displayCharString(img_,10,10,"Click on the window to select the current image",vpColor::red);
vpDisplay::flush(img_);
if(pause_image_){
pause_image_= false;
}
else{
return;
}
}
pose.clearPoint();
calib.clearPoint();
vpImagePoint ip;
//lets the user select keypoints
for(unsigned int i=0;i<selected_points_.size();i++){
try{
vpDot2 d;
d.setGrayLevelPrecision(gray_level_precision);
d.setSizePrecision(size_precision);
ROS_INFO("Click on point %d",i+1);
vpDisplay::displayRectangle(img_,0,0,img_.getWidth(),15,vpColor::black,true);
vpDisplay::displayCharString(img_,10,10,boost::str(boost::format("click on point %1%") % (i+1)).c_str(),vpColor::red);
vpDisplay::flush(img_);
while(ros::ok() && !vpDisplay::getClick(img_,ip,false));
d.initTracking(img_, ip);
ip.set_ij(d.getCog().get_i(),d.getCog().get_j());
double x=0,y=0;
vpPixelMeterConversion::convertPoint(cam_, ip, x, y);
selected_points_[i].set_x(x);
selected_points_[i].set_y(y);
pose.addPoint(selected_points_[i]);
calib.addPoint(selected_points_[i].get_oX(),selected_points_[i].get_oY(),selected_points_[i].get_oZ(), ip);
vpDisplay::displayCross(img_, d.getCog(), 10, vpColor::red);
vpDisplay::flush(img_);
}catch(vpTrackingException e){
ROS_ERROR("Failed to init point");
}
}
vpHomogeneousMatrix cMo;
pose.computePose(vpPose::LAGRANGE, cMo) ;
pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
vpHomogeneousMatrix cMoTmp = cMo;
vpCameraParameters camTmp = cam_;
//compute local calibration to match the calibration grid with the image
try{
calib.computeCalibration(vpCalibration::CALIB_VIRTUAL_VS,cMoTmp,camTmp,false);
ROS_DEBUG_STREAM("cMo="<<std::endl <<cMoTmp<<std::endl);
ROS_DEBUG_STREAM("cam="<<std::endl <<camTmp<<std::endl);
//project all points and track their corresponding image location
for (std::vector<vpPoint>::iterator model_point_iter= model_points_.begin();
model_point_iter!= model_points_.end() ;
model_point_iter++){
//project each model point into image according to current calibration
vpColVector _cP, _p ;
model_point_iter->changeFrame(cMoTmp,_cP) ;
model_point_iter->projection(_cP,_p) ;
vpMeterPixelConversion::convertPoint(camTmp,_p[0],_p[1], ip);
if (10 < ip.get_u() && ip.get_u() < img_.getWidth()-10 &&
10 < ip.get_v() && ip.get_v() < img_.getHeight()-10) {
try {
//track the projected point, look for match
vpDot2 md;
md.setGrayLevelPrecision(gray_level_precision);
md.setSizePrecision(size_precision);
md.initTracking(img_, ip);
if(!ros::ok())
return;
vpRect bbox = md.getBBox();
vpImagePoint cog = md.getCog();
if(bbox.getLeft()<5 || bbox.getRight()>(double)img_.getWidth()-5 ||
bbox.getTop()<5 || bbox.getBottom()>(double)img_.getHeight()-5||
vpMath::abs(ip.get_u() - cog.get_u()) > 10 ||
vpMath::abs(ip.get_v() - cog.get_v()) > 10){
ROS_DEBUG("tracking failed[suspicious point location].");
}else{
//point matches
double x=0, y=0;
vpPixelMeterConversion::convertPoint(camTmp, cog, x, y) ;
model_point_iter->set_x(x) ;
model_point_iter->set_y(y) ;
md.display(img_,vpColor::yellow, 2);
visp_camera_calibration::CalibPoint cp;
cp.i = cog.get_i();
cp.j = cog.get_j();
cp.X = model_point_iter->get_oX();
cp.Y = model_point_iter->get_oY();
cp.Z = model_point_iter->get_oZ();
calib_all_points.points.push_back(cp);
model_point_iter->display(img_,cMoTmp,camTmp) ;
loop_rate.sleep(); //To avoid refresh problems
vpDisplay::flush(img_);
}
} catch(...){
ROS_DEBUG("tracking failed.");
}
} else {
ROS_DEBUG("bad projection.");
}
}
ROS_INFO("Left click on the interface window to continue, right click to restart");
vpDisplay::displayRectangle(img_,0,0,img_.getWidth(),15,vpColor::black,true);
vpDisplay::displayCharString(img_,10,10,"Left click on the interface window to continue, right click to restart",vpColor::red);
vpDisplay::flush(img_);
vpMouseButton::vpMouseButtonType btn;
while(ros::ok() && !vpDisplay::getClick(img_,ip,btn, false));
if(btn==vpMouseButton::button1)
point_correspondence_publisher_.publish(calib_all_points);
else{
rawImageCallback(image);
return;
}
}catch(...){
ROS_ERROR("calibration failed.");
}
}
geometry_msgs::PoseWithCovarianceStamped PredictionModel::update( double twist_x, double twist_y, double twist_z )
{
//add system noise for dispersing the model if no value was received.
// base the predicted movement of the model on the current linear twist
// the velocity is divided by the refresh frequency as the velocity
// is expressed in m.s-1
MatrixWrapper::ColumnVector vel(3); vel = 0;
vel(1) = twist_x / refresh_frequency_;
vel(2) = twist_y / refresh_frequency_;
vel(3) = twist_z / refresh_frequency_;
//we have a new measurement waiting to be processed
if( !meas_used_ )
{
ROS_DEBUG_STREAM("New measurement = " << measurement_ << " / vel: " << vel);
kalman_filter_->Update( system_model_.get(), vel,
measurement_model_.get(),
measurement_);
meas_used_ = true;
}
else
{
//no measurement only dispersion
ROS_DEBUG_STREAM("Dispersion model, velocities: " << vel);
kalman_filter_->Update(system_model_.get(), vel);
}
//get the result
posterior_ = kalman_filter_->PostGet();
ROS_DEBUG_STREAM("Object is probably at: " << posterior_->ExpectedValueGet() << " \n"
<< " -> Covariance of: " << posterior_->CovarianceGet());
results_.pose.pose.position.x = posterior_->ExpectedValueGet()(1);
results_.pose.pose.position.y = posterior_->ExpectedValueGet()(2);
results_.pose.pose.position.z = posterior_->ExpectedValueGet()(3);
double x, y, z;
x = posterior_->ExpectedValueGet()(1);
y = posterior_->ExpectedValueGet()(2);
z = posterior_->ExpectedValueGet()(3);
if( (x != x) | (y != y) | (z != z) )
{
ROS_WARN_STREAM("There was a problem, we predicted some NaN: " << x << " " << y << " " << z <<" / resetting filter." );
reset_model_();
timer_ = nh_.createTimer(ros::Duration(0.1), &PredictionModel::reset, this, true);
//hack to mark it as failed
results_.header.seq = 0;
return results_;
}
else
{
//hack to mark it as success
results_.header.seq = 1;
}
//TODO compute orientation as well?
results_.pose.pose.orientation.x = 0.0;
results_.pose.pose.orientation.y = 0.0;
results_.pose.pose.orientation.z = 0.0;
results_.pose.pose.orientation.w = 1.0;
//The covariance from the pose is a 6x6 matrix
// but we've only got a 3x3 matrix as we're estimating the position only
results_.pose.covariance[0] = posterior_->CovarianceGet()(1,1);
results_.pose.covariance[1] = posterior_->CovarianceGet()(1,2);
results_.pose.covariance[2] = posterior_->CovarianceGet()(1,3);
results_.pose.covariance[6] = posterior_->CovarianceGet()(2,1);
results_.pose.covariance[7] = posterior_->CovarianceGet()(2,2);
results_.pose.covariance[8] = posterior_->CovarianceGet()(2,3);
results_.pose.covariance[12] = posterior_->CovarianceGet()(3,1);
results_.pose.covariance[13] = posterior_->CovarianceGet()(3,2);
results_.pose.covariance[14] = posterior_->CovarianceGet()(3,3);
return results_;
}
/** 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 NaoqiCameraDriver::reconfig(Config &newconfig, uint32_t level)
{
// Do not run concurrently with poll().
boost::mutex::scoped_lock scopedLock(reconfiguration_mutex_);
ROS_DEBUG("dynamic reconfigure level 0x%x", level);
// resolve frame ID using tf_prefix parameter
if (newconfig.source == kTopCamera)
frame_id_ = "CameraTop_frame";
else if (newconfig.source == kBottomCamera)
frame_id_ = "CameraBottom_frame";
else {
ROS_ERROR("Unknown video source! (neither top nor bottom camera).");
frame_id_ = "camera";
}
string tf_prefix = tf::getPrefixParam(priv_nh_);
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
frame_id_ = tf::resolve(tf_prefix, frame_id_);
if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
{
// must close the device before updating these parameters
closeCamera(); // state_ --> CLOSED
}
if (config_.camera_info_url != newconfig.camera_info_url)
{
// set the new URL and load CameraInfo (if any) from it
if (cinfo_->validateURL(newconfig.camera_info_url))
{
cinfo_->loadCameraInfo(newconfig.camera_info_url);
}
else
{
// new URL not valid, use the old one
newconfig.camera_info_url = config_.camera_info_url;
}
}
if (state_ != Driver::CLOSED) // openCamera() succeeded?
{
if (config_.auto_exposition != newconfig.auto_exposition)
camera_proxy_->setParam(kCameraAutoExpositionID, newconfig.auto_exposition);
if (config_.auto_exposure_algo != newconfig.auto_exposure_algo)
camera_proxy_->setParam(kCameraExposureAlgorithmID, newconfig.auto_exposure_algo);
if (config_.exposure != newconfig.exposure) {
newconfig.auto_exposition = 0;
camera_proxy_->setParam(kCameraAutoExpositionID, 0);
camera_proxy_->setParam(kCameraExposureID, newconfig.exposure);
}
if (config_.gain != newconfig.gain) {
newconfig.auto_exposition = 0;
camera_proxy_->setParam(kCameraAutoExpositionID, 0);
camera_proxy_->setParam(kCameraGainID, newconfig.gain);
}
if (config_.brightness != newconfig.brightness) {
newconfig.auto_exposition = 1;
camera_proxy_->setParam(kCameraAutoExpositionID, 1);
camera_proxy_->setParam(kCameraBrightnessID, newconfig.brightness);
}
if (config_.contrast != newconfig.contrast)
camera_proxy_->setParam(kCameraContrastID, newconfig.contrast);
if (config_.saturation != newconfig.saturation)
camera_proxy_->setParam(kCameraSaturationID, newconfig.saturation);
if (config_.hue != newconfig.hue)
camera_proxy_->setParam(kCameraHueID, newconfig.hue);
if (config_.sharpness != newconfig.sharpness)
camera_proxy_->setParam(kCameraSharpnessID, newconfig.sharpness);
if (config_.auto_white_balance != newconfig.auto_white_balance)
camera_proxy_->setParam(kCameraAutoWhiteBalanceID, newconfig.auto_white_balance);
if (config_.white_balance != newconfig.white_balance) {
newconfig.auto_white_balance = 0;
camera_proxy_->setParam(kCameraAutoWhiteBalanceID, 0);
camera_proxy_->setParam(kCameraWhiteBalanceID, newconfig.white_balance);
}
}
config_ = newconfig; // save new parameters
real_frame_rate_ = ros::Rate(newconfig.frame_rate);
ROS_DEBUG_STREAM("[" << camera_name_
<< "] reconfigured: frame_id " << frame_id_
<< ", camera_info_url " << newconfig.camera_info_url);
}
void planning_environment::KinematicModelStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
{
//bool change = !have_joint_state_;
planning_models::KinematicState state(kmodel_);
current_joint_values_lock_.lock();
state.setKinematicState(current_joint_state_map_);
static bool first_time = true;
unsigned int n = joint_state->name.size();
if (joint_state->name.size() != joint_state->position.size())
{
ROS_ERROR("Planning environment received invalid joint state");
current_joint_values_lock_.unlock();
return;
}
std::map<std::string, double> joint_state_map;
for (unsigned int i = 0 ; i < n ; ++i)
{
joint_state_map[joint_state->name[i]] = joint_state->position[i];
}
std::vector<planning_models::KinematicState::JointState*>& joint_state_vector = state.getJointStateVector();
for(std::vector<planning_models::KinematicState::JointState*>::iterator it = joint_state_vector.begin();
it != joint_state_vector.end();
it++) {
bool tfSets = false;
bool joint_state_sets = false;
//see if we need to update any transforms
std::string parent_frame_id = (*it)->getParentFrameId();
std::string child_frame_id = (*it)->getChildFrameId();
if(!parent_frame_id.empty() && !child_frame_id.empty()) {
std::string err;
ros::Time tm;
tf::StampedTransform transf;
bool ok = false;
if (tf_->getLatestCommonTime(parent_frame_id, child_frame_id, tm, &err) == tf::NO_ERROR) {
ok = true;
try
{
tf_->lookupTransform(parent_frame_id, child_frame_id, tm, transf);
}
catch(tf::TransformException& ex)
{
ROS_ERROR("Unable to lookup transform from %s to %s. Exception: %s", parent_frame_id.c_str(), child_frame_id.c_str(), ex.what());
ok = false;
}
} else {
ROS_DEBUG("Unable to lookup transform from %s to %s: no common time.", parent_frame_id.c_str(), child_frame_id.c_str());
ok = false;
}
if(ok) {
tfSets = (*it)->setJointStateValues(transf);
if(tfSets) {
const std::vector<std::string>& joint_state_names = (*it)->getJointStateNameOrder();
for(std::vector<std::string>::const_iterator it = joint_state_names.begin();
it != joint_state_names.end();
it++) {
last_joint_update_[*it] = tm;
}
}
// tf::Transform transf = getKinematicModel()->getRoot()->variable_transform;
// ROS_INFO_STREAM("transform is to " << transf.getRotation().x() << " "
// << transf.getRotation().y() << " z " << transf.getRotation().z()
// << " w " << transf.getRotation().w());
have_pose_ = tfSets;
last_pose_update_ = tm;
}
}
//now we update from joint state
joint_state_sets = (*it)->setJointStateValues(joint_state_map);
if(joint_state_sets) {
const std::vector<std::string>& joint_state_names = (*it)->getJointStateNameOrder();
for(std::vector<std::string>::const_iterator it = joint_state_names.begin();
it != joint_state_names.end();
it++) {
last_joint_update_[*it] = joint_state->header.stamp;
}
}
}
state.updateKinematicLinks();
state.getKinematicStateValues(current_joint_state_map_);
if(allJointsUpdated()) {
have_joint_state_ = true;
last_joint_state_update_ = joint_state->header.stamp;
if(!joint_state_map_cache_.empty()) {
if(joint_state->header.stamp-joint_state_map_cache_.back().first > ros::Duration(joint_state_cache_allowed_difference_)) {
ROS_DEBUG_STREAM("Introducing joint state cache sparsity time of " << (joint_state->header.stamp-joint_state_map_cache_.back().first).toSec());
}
}
joint_state_map_cache_.push_back(std::pair<ros::Time, std::map<std::string, double> >(joint_state->header.stamp,
current_joint_state_map_));
}
if(have_joint_state_) {
while(1) {
if(joint_state_map_cache_.empty()) {
ROS_WARN("Empty joint state map cache");
break;
}
if((ros::Time::now()-joint_state_map_cache_.front().first) > ros::Duration(joint_state_cache_time_)) {
joint_state_map_cache_.pop_front();
} else {
break;
}
}
}
first_time = false;
if(!allJointsUpdated(ros::Duration(1.0))) {
if(!printed_out_of_date_) {
ROS_WARN_STREAM("Got joint state update but did not update some joints for more than 1 second. Turn on DEBUG for more info");
printed_out_of_date_ = true;
}
} else {
printed_out_of_date_ = false;
}
if(on_state_update_callback_ != NULL) {
on_state_update_callback_(joint_state);
}
current_joint_values_lock_.unlock();
}
void DensoArmNode::go_to_arm_pose(const denso_msgs::MoveArmPoseGoalConstPtr& goal)
{
int success = denso_msgs::MoveArmPoseResult::SUCCESS;
//rate at which we'll send the demands to the arm
ros::Rate move_arm_rate( goal->rate );
//for checking the time out
ros::Duration time_left( goal->time_out );
ros::Time start_time = ros::Time::now();
while( node_.ok() )
{
if( move_arm_pose_server_->isPreemptRequested() || !ros::ok() )
{
ROS_INFO("Move Denso arm to pose preempted.");
move_arm_pose_server_->setPreempted();
success = denso_msgs::MoveArmPoseResult::PREEMPTED;
break;
}
//Compute pose from goal
geometry_msgs::Pose pose_tmp( goal->goal );
Pose pose_goal = geometry_pose_to_denso_pose( pose_tmp );
ROS_DEBUG_STREAM(" RPY : " << pose_goal.roll << " / " << pose_goal.pitch << " / " << pose_goal.yaw);
bool locked = false;
for( unsigned int i=0; i < 1000; ++i)
{
if( denso_mutex.try_lock() )
{
locked = true;
break;
}
usleep(10000);
}
if( locked )
{
if( !simulated )
{
denso_arm_->set_speed( goal->speed );
if( denso_arm_->send_cartesian_position( pose_goal ) )
{
denso_mutex.unlock();
break; //We reached the target -> SUCCESS
}
}
else
{
usleep( 500000 );
denso_mutex.unlock();
break;
}
}
else
{
ROS_WARN("Couldn't send the target to the arm.");
}
//publish feedback
time_left -= (ros::Time::now() - start_time);
move_arm_pose_feedback_.time_left = time_left;
move_arm_pose_server_->publishFeedback( move_arm_pose_feedback_ );
//check if timedout
if( time_left.toSec() <= 0.0 )
{
success = denso_msgs::MoveArmPoseResult::TIMED_OUT;
break;
}
//loop at the rate the user asked for
move_arm_rate.sleep();
}
if( success == denso_msgs::MoveArmPoseResult::SUCCESS)
{
move_arm_pose_result_.val = denso_msgs::MoveArmPoseResult::SUCCESS;
move_arm_pose_server_->setSucceeded( move_arm_pose_result_ );
}
else
{
//failed
move_arm_pose_result_.val = success;
move_arm_pose_server_->setAborted( move_arm_pose_result_ );
}
}
void RosReconfigure_callback(Config &config, uint32_t level)
{
int changedAcquire;
int changedAcquisitionFrameRate;
int changedExposureAuto;
int changedGainAuto;
int changedExposureTimeAbs;
int changedGain;
int changedAcquisitionMode;
int changedTriggerMode;
int changedTriggerSource;
int changedSoftwarerate;
int changedFrameid;
int changedFocusPos;
int changedMtu;
std::string tf_prefix = tf::getPrefixParam(*global.phNode);
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
if (config.frame_id == "")
config.frame_id = "camera";
// Find what the user changed.
changedAcquire = (global.config.Acquire != config.Acquire);
changedAcquisitionFrameRate = (global.config.AcquisitionFrameRate != config.AcquisitionFrameRate);
changedExposureAuto = (global.config.ExposureAuto != config.ExposureAuto);
changedExposureTimeAbs = (global.config.ExposureTimeAbs != config.ExposureTimeAbs);
changedGainAuto = (global.config.GainAuto != config.GainAuto);
changedGain = (global.config.Gain != config.Gain);
changedAcquisitionMode = (global.config.AcquisitionMode != config.AcquisitionMode);
changedTriggerMode = (global.config.TriggerMode != config.TriggerMode);
changedTriggerSource = (global.config.TriggerSource != config.TriggerSource);
changedSoftwarerate = (global.config.softwaretriggerrate != config.softwaretriggerrate);
changedFrameid = (global.config.frame_id != config.frame_id);
changedFocusPos = (global.config.FocusPos != config.FocusPos);
changedMtu = (global.config.mtu != config.mtu);
// Limit params to legal values.
config.AcquisitionFrameRate = CLIP(config.AcquisitionFrameRate, global.configMin.AcquisitionFrameRate, global.configMax.AcquisitionFrameRate);
config.ExposureTimeAbs = CLIP(config.ExposureTimeAbs, global.configMin.ExposureTimeAbs, global.configMax.ExposureTimeAbs);
config.Gain = CLIP(config.Gain, global.configMin.Gain, global.configMax.Gain);
config.FocusPos = CLIP(config.FocusPos, global.configMin.FocusPos, global.configMax.FocusPos);
config.frame_id = tf::resolve(tf_prefix, config.frame_id);
// Adjust other controls dependent on what the user changed.
if (changedExposureTimeAbs || changedGainAuto || ((changedAcquisitionFrameRate || changedGain || changedFrameid
|| changedAcquisitionMode || changedTriggerSource || changedSoftwarerate) && config.ExposureAuto=="Once"))
config.ExposureAuto = "Off";
if (changedGain || changedExposureAuto || ((changedAcquisitionFrameRate || changedExposureTimeAbs || changedFrameid
|| changedAcquisitionMode || changedTriggerSource || changedSoftwarerate) && config.GainAuto=="Once"))
config.GainAuto = "Off";
if (changedAcquisitionFrameRate)
config.TriggerMode = "Off";
// Find what changed for any reason.
changedAcquire = (global.config.Acquire != config.Acquire);
changedAcquisitionFrameRate = (global.config.AcquisitionFrameRate != config.AcquisitionFrameRate);
changedExposureAuto = (global.config.ExposureAuto != config.ExposureAuto);
changedExposureTimeAbs = (global.config.ExposureTimeAbs != config.ExposureTimeAbs);
changedGainAuto = (global.config.GainAuto != config.GainAuto);
changedGain = (global.config.Gain != config.Gain);
changedAcquisitionMode = (global.config.AcquisitionMode != config.AcquisitionMode);
changedTriggerMode = (global.config.TriggerMode != config.TriggerMode);
changedTriggerSource = (global.config.TriggerSource != config.TriggerSource);
changedSoftwarerate = (global.config.softwaretriggerrate != config.softwaretriggerrate);
changedFrameid = (global.config.frame_id != config.frame_id);
changedFocusPos = (global.config.FocusPos != config.FocusPos);
changedMtu = (global.config.mtu != config.mtu);
// Set params into the camera.
if (changedExposureTimeAbs)
{
if (global.isImplementedExposureTimeAbs)
{
ROS_INFO ("Set ExposureTimeAbs = %f", config.ExposureTimeAbs);
arv_device_set_float_feature_value (global.pDevice, "ExposureTimeAbs", config.ExposureTimeAbs);
}
else
ROS_INFO ("Camera does not support ExposureTimeAbs.");
}
if (changedGain)
{
if (global.isImplementedGain)
{
ROS_INFO ("Set gain = %f", config.Gain);
//arv_device_set_integer_feature_value (global.pDevice, "GainRaw", config.GainRaw);
arv_camera_set_gain (global.pCamera, config.Gain);
}
else
ROS_INFO ("Camera does not support Gain or GainRaw.");
}
if (changedExposureAuto)
{
if (global.isImplementedExposureAuto && global.isImplementedExposureTimeAbs)
{
ROS_INFO ("Set ExposureAuto = %s", config.ExposureAuto.c_str());
arv_device_set_string_feature_value (global.pDevice, "ExposureAuto", config.ExposureAuto.c_str());
if (config.ExposureAuto=="Once")
{
ros::Duration(2.0).sleep();
config.ExposureTimeAbs = arv_device_get_float_feature_value (global.pDevice, "ExposureTimeAbs");
ROS_INFO ("Get ExposureTimeAbs = %f", config.ExposureTimeAbs);
config.ExposureAuto = "Off";
}
}
else
ROS_INFO ("Camera does not support ExposureAuto.");
}
if (changedGainAuto)
{
if (global.isImplementedGainAuto && global.isImplementedGain)
{
ROS_INFO ("Set GainAuto = %s", config.GainAuto.c_str());
arv_device_set_string_feature_value (global.pDevice, "GainAuto", config.GainAuto.c_str());
if (config.GainAuto=="Once")
{
ros::Duration(2.0).sleep();
//config.GainRaw = arv_device_get_integer_feature_value (global.pDevice, "GainRaw");
config.Gain = arv_camera_get_gain (global.pCamera);
ROS_INFO ("Get Gain = %f", config.Gain);
config.GainAuto = "Off";
}
}
else
ROS_INFO ("Camera does not support GainAuto.");
}
if (changedAcquisitionFrameRate)
{
if (global.isImplementedAcquisitionFrameRate)
{
ROS_INFO ("Set %s = %f", global.keyAcquisitionFrameRate, config.AcquisitionFrameRate);
arv_device_set_float_feature_value (global.pDevice, global.keyAcquisitionFrameRate, config.AcquisitionFrameRate);
}
else
ROS_INFO ("Camera does not support AcquisitionFrameRate.");
}
if (changedTriggerMode)
{
if (global.isImplementedTriggerMode)
{
ROS_INFO ("Set TriggerMode = %s", config.TriggerMode.c_str());
arv_device_set_string_feature_value (global.pDevice, "TriggerMode", config.TriggerMode.c_str());
}
else
ROS_INFO ("Camera does not support TriggerMode.");
}
if (changedTriggerSource)
{
if (global.isImplementedTriggerSource)
{
ROS_INFO ("Set TriggerSource = %s", config.TriggerSource.c_str());
arv_device_set_string_feature_value (global.pDevice, "TriggerSource", config.TriggerSource.c_str());
}
else
ROS_INFO ("Camera does not support TriggerSource.");
}
if ((changedTriggerMode || changedTriggerSource || changedSoftwarerate) && config.TriggerMode=="On" && config.TriggerSource=="Software")
{
if (global.isImplementedAcquisitionFrameRate)
{
// The software rate is limited by the camera's internal framerate. Bump up the camera's internal framerate if necessary.
config.AcquisitionFrameRate = global.configMax.AcquisitionFrameRate;
ROS_INFO ("Set %s = %f", global.keyAcquisitionFrameRate, config.AcquisitionFrameRate);
arv_device_set_float_feature_value (global.pDevice, global.keyAcquisitionFrameRate, config.AcquisitionFrameRate);
}
}
if (changedTriggerSource || changedSoftwarerate)
{
// Recreate the software trigger callback.
if (global.idSoftwareTriggerTimer)
{
g_source_remove(global.idSoftwareTriggerTimer);
global.idSoftwareTriggerTimer = 0;
}
if (!strcmp(config.TriggerSource.c_str(),"Software"))
{
ROS_INFO ("Set softwaretriggerrate = %f", 1000.0/ceil(1000.0 / config.softwaretriggerrate));
// Turn on software timer callback.
global.idSoftwareTriggerTimer = g_timeout_add ((guint)ceil(1000.0 / config.softwaretriggerrate), SoftwareTrigger_callback, global.pCamera);
}
}
if (changedFocusPos)
{
if (global.isImplementedFocusPos)
{
ROS_INFO ("Set FocusPos = %d", config.FocusPos);
arv_device_set_integer_feature_value(global.pDevice, "FocusPos", config.FocusPos);
ros::Duration(1.0).sleep();
config.FocusPos = arv_device_get_integer_feature_value(global.pDevice, "FocusPos");
ROS_INFO ("Get FocusPos = %d", config.FocusPos);
}
else
ROS_INFO ("Camera does not support FocusPos.");
}
if (changedMtu)
{
if (global.isImplementedMtu)
{
ROS_INFO ("Set mtu = %d", config.mtu);
arv_device_set_integer_feature_value(global.pDevice, "GevSCPSPacketSize", config.mtu);
ros::Duration(1.0).sleep();
config.mtu = arv_device_get_integer_feature_value(global.pDevice, "GevSCPSPacketSize");
ROS_INFO ("Get mtu = %d", config.mtu);
}
else
ROS_INFO ("Camera does not support mtu (i.e. GevSCPSPacketSize).");
}
if (changedAcquisitionMode)
{
if (global.isImplementedAcquisitionMode)
{
ROS_INFO ("Set AcquisitionMode = %s", config.AcquisitionMode.c_str());
arv_device_set_string_feature_value (global.pDevice, "AcquisitionMode", config.AcquisitionMode.c_str());
ROS_INFO("AcquisitionStop");
arv_device_execute_command (global.pDevice, "AcquisitionStop");
ROS_INFO("AcquisitionStart");
arv_device_execute_command (global.pDevice, "AcquisitionStart");
}
else
ROS_INFO ("Camera does not support AcquisitionMode.");
}
if (changedAcquire)
{
if (config.Acquire)
{
ROS_INFO("AcquisitionStart");
arv_device_execute_command (global.pDevice, "AcquisitionStart");
}
else
{
ROS_INFO("AcquisitionStop");
arv_device_execute_command (global.pDevice, "AcquisitionStop");
}
}
global.config = config;
} // RosReconfigure_callback()
void printTransform(const char* name, const tf::Transform t) {
ROS_DEBUG_STREAM(name << ": Translation " << t.getOrigin().x() << " " << t.getOrigin().y() << " " << t.getOrigin().z());
ROS_DEBUG_STREAM(name << ": Rotation " << t.getRotation().getX() << " " << t.getRotation().getY() << " " << t.getRotation().getZ() << " " << t.getRotation().getW());
}
/**
* @brief Initialises the node.
*/
bool KeyOpCore::init()
{
ros::NodeHandle nh("~");
name = nh.getUnresolvedNamespace();
/*********************
** Parameters
**********************/
nh.getParam("linear_vel_step", linear_vel_step);
nh.getParam("linear_vel_max", linear_vel_max);
nh.getParam("angular_vel_step", angular_vel_step);
nh.getParam("angular_vel_max", angular_vel_max);
nh.getParam("wait_for_connection", wait_for_connection_);
ROS_INFO_STREAM("KeyOpCore : using linear vel step [" << linear_vel_step << "].");
ROS_INFO_STREAM("KeyOpCore : using linear vel max [" << linear_vel_max << "].");
ROS_INFO_STREAM("KeyOpCore : using angular vel step [" << angular_vel_step << "].");
ROS_INFO_STREAM("KeyOpCore : using angular vel max [" << angular_vel_max << "].");
/*********************
** Subscribers
**********************/
keyinput_subscriber = nh.subscribe("teleop", 1, &KeyOpCore::remoteKeyInputReceived, this);
/*********************
** Publishers
**********************/
velocity_publisher_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
motor_power_publisher_ = nh.advertise<kobuki_msgs::MotorPower>("motor_power", 1);
/*********************
** Velocities
**********************/
cmd->linear.x = 0.0;
cmd->linear.y = 0.0;
cmd->linear.z = 0.0;
cmd->angular.x = 0.0;
cmd->angular.y = 0.0;
cmd->angular.z = 0.0;
/*********************
** Wait for connection
**********************/
if (!wait_for_connection_)
{
return true;
}
ecl::MilliSleep millisleep;
int count = 0;
bool connected = false;
while (!connected)
{
if (motor_power_publisher_.getNumSubscribers() > 0)
{
connected = true;
break;
}
if (count == 6)
{
connected = false;
break;
}
else
{
ROS_WARN_STREAM("KeyOp: could not connect, trying again after 500ms...");
try
{
millisleep(500);
}
catch (ecl::StandardException e)
{
ROS_ERROR_STREAM("Waiting has been interrupted.");
ROS_DEBUG_STREAM(e.what());
return false;
}
++count;
}
}
if (!connected)
{
ROS_ERROR("KeyOp: could not connect.");
ROS_ERROR("KeyOp: check remappings for enable/disable topics).");
}
else
{
kobuki_msgs::MotorPower power_cmd;
power_cmd.state = kobuki_msgs::MotorPower::ON;
motor_power_publisher_.publish(power_cmd);
ROS_INFO("KeyOp: connected.");
power_status = true;
}
// start keyboard input thread
thread.start(&KeyOpCore::keyboardInputLoop, *this);
return true;
}
// This example opens the serial port and sends a request 'R' at 1Hz and waits for a reply.
int main(int argc, char** argv)
{
ros::init(argc, argv, "imu_tf_publisher");
ros::NodeHandle n;
cereal::CerealPort device;
char reply[REPLY_SIZE];
char* tempData;
int read_len = 0;
int data_counter = 0;
double imu_x, imu_y, imu_z, imu_w = 0.0;
char* head_ID = "4-2";
char* right_hand_ID = "6-1";
char* left_hand_ID = "6-3";
// Change the next line according to your port name and baud rate
try{ device.open("/dev/ttyUSB0", 115200); }
catch(cereal::Exception& e)
{
ROS_FATAL("Failed to open the serial port!!!");
ROS_BREAK();
}
ROS_INFO("The serial port is opened.");
ros::Rate r(500);
tf::TransformBroadcaster br;
tf::TransformListener ls;
std::string transformString1;
std::string transformString2;
tf::StampedTransform stamped_transform;
while(ros::ok())
{
// Send 'R' over the serial port
//device.write("3");
// Get the reply, the last value is the timeout in ms
try{ read_len = device.read(reply, REPLY_SIZE, TIMEOUT); }
catch(cereal::TimeoutException& e)
{
ROS_ERROR(e.what());
device.open("/dev/ttyUSB0", 115200);
}
if(read_len > 0)
{
tempData = strtok(reply, ",");
data_counter = 0;
if(tempData = strtok(NULL, ","))
{
data_counter++;
imu_x = (double)atoi(tempData)/10000.0;
}
if(tempData = strtok(NULL, ","))
{
data_counter++;
imu_y = (double)atoi(tempData)/10000.0;
}
if(tempData = strtok(NULL, ","))
{
data_counter++;
imu_z = (double)atoi(tempData)/10000.0;
}
if(tempData = strtok(NULL, ","))
{
data_counter++;
imu_w = (double)atoi(tempData)/10000.0;
}
if(data_counter == 4 && strncmp(reply, head_ID, 3)==0)
{
transformString1 = "neck";
transformString2 = "head";
try
{
ls.waitForTransform(transformString1,transformString2,ros::Time(0),ros::Duration(1.0));
ls.lookupTransform(transformString1,transformString2,ros::Time(0),stamped_transform);
}
catch(tf::TransformException const &ex)
{
ROS_DEBUG_STREAM(ex.what());
ROS_WARN_STREAM("Couldn't get neck to head transform!");
}
stamped_transform.setRotation(tf::Quaternion(imu_x,imu_y,imu_z,imu_w));
br.sendTransform(tf::StampedTransform(stamped_transform, ros::Time::now(),stamped_transform.frame_id_,"imu_head"));
ROS_INFO("Got this reply for head: x : %f ,y : %f ,z : %f ,w : %f", imu_x, imu_y, imu_z, imu_w);
}
else if(data_counter == 4 && strncmp(reply, right_hand_ID, 3)==0)
{
transformString1 = "right_elbow";
transformString2 = "right_hand";
try
{
ls.waitForTransform(transformString1,transformString2,ros::Time(0),ros::Duration(1.0));
ls.lookupTransform(transformString1,transformString2,ros::Time(0),stamped_transform);
}
catch(tf::TransformException const &ex)
{
ROS_DEBUG_STREAM(ex.what());
ROS_WARN_STREAM("Couldn't get right elbow to right hand transform!");
}
stamped_transform.setRotation(tf::Quaternion(imu_x,imu_y,imu_z,imu_w));
br.sendTransform(tf::StampedTransform(stamped_transform, ros::Time::now(),stamped_transform.frame_id_,"imu_right_hand"));
ROS_INFO("Got this reply for right hand: x : %f ,y : %f ,z : %f ,w : %f", imu_x, imu_y, imu_z, imu_w);
}
else if(data_counter == 4 && strncmp(reply, left_hand_ID, 3)==0)
{
transformString1 = "left_elbow";
transformString2 = "left_hand";
try
{
ls.waitForTransform(transformString1,transformString2,ros::Time(0),ros::Duration(1.0));
ls.lookupTransform(transformString1,transformString2,ros::Time(0),stamped_transform);
}
catch(tf::TransformException const &ex)
{
ROS_DEBUG_STREAM(ex.what());
ROS_WARN_STREAM("Couldn't get left elbow to left hand transform!");
}
//transform.setOrigin(tf::Vector3(0.0,0.0,0.0));
stamped_transform.setRotation(tf::Quaternion(imu_x,imu_y,imu_z,imu_w));
br.sendTransform(tf::StampedTransform(stamped_transform, ros::Time::now(),stamped_transform.frame_id_,"imu_left_hand"));
ROS_INFO("Got this reply for left hand: x : %f ,y : %f ,z : %f ,w : %f", imu_x, imu_y, imu_z, imu_w);
}
read_len = 0;
}
ros::spinOnce();
r.sleep();
}
}
/* ------------------------ JointModelGroup ------------------------ */
planning_models::KinematicModel::JointModelGroup::JointModelGroup(const std::string& group_name,
const std::vector<const JointModel*> &group_joints,
const std::vector<const JointModel*> &fixed_group_joints,
const KinematicModel* parent_model) :
name_(group_name)
{
ROS_DEBUG_STREAM("Group " << group_name);
joint_model_vector_ = group_joints;
fixed_joint_model_vector_ = fixed_group_joints;
joint_model_name_vector_.resize(group_joints.size());
ROS_DEBUG_STREAM("Joints:");
for (unsigned int i = 0 ; i < group_joints.size() ; ++i)
{
ROS_DEBUG_STREAM(group_joints[i]->getName());
joint_model_name_vector_[i] = group_joints[i]->getName();
joint_model_map_[group_joints[i]->getName()] = group_joints[i];
}
ROS_DEBUG_STREAM("Fixed joints:");
for (unsigned int i = 0 ; i < fixed_joint_model_vector_.size() ; ++i)
{
ROS_DEBUG_STREAM(fixed_joint_model_vector_[i]->getName());
}
std::vector<const JointModel*> all_joints = group_joints;
all_joints.insert(all_joints.end(), fixed_group_joints.begin(), fixed_group_joints.end());
//now we need to find all the set of joints within this group
//that root distinct subtrees
std::map<std::string, bool> is_root;
for (unsigned int i = 0 ; i < all_joints.size() ; ++i)
{
bool found = false;
const JointModel *joint = all_joints[i];
//if we find that an ancestor is also in the group, then the joint is not a root
while (joint->getParentLinkModel() != NULL)
{
joint = joint->getParentLinkModel()->getParentJointModel();
if (hasJointModel(joint->name_))
{
found = true;
}
}
if(!found) {
joint_roots_.push_back(all_joints[i]);
is_root[all_joints[i]->getName()] = true;
} else {
is_root[all_joints[i]->getName()] = false;
}
}
//now we need to make another pass for group links
std::set<const LinkModel*> group_links_set;
for(unsigned int i = 0; i < all_joints.size(); i++) {
const JointModel *joint = all_joints[i];
//push children in directly
group_links_set.insert(joint->getChildLinkModel());
while (joint->getParentLinkModel() != NULL)
{
if(is_root.find(joint->getName()) != is_root.end() &&
is_root[joint->getName()]) {
break;
}
group_links_set.insert(joint->getParentLinkModel());
joint = joint->getParentLinkModel()->getParentJointModel();
}
}
ROS_DEBUG("Group links:");
for(std::set<const LinkModel*>::iterator it = group_links_set.begin();
it != group_links_set.end();
it++) {
group_link_model_vector_.push_back(*it);
ROS_DEBUG_STREAM((*it)->getName());
}
//these subtrees are distinct, so we can stack their updated links on top of each other
for (unsigned int i = 0 ; i < joint_roots_.size() ; ++i)
{
std::vector<const LinkModel*> links;
parent_model->getChildLinkModels(joint_roots_[i], links);
updated_link_model_vector_.insert(updated_link_model_vector_.end(), links.begin(), links.end());
}
}
// Processes the point cloud with OpenCV using the PCL cluster indices
void processClusters( const std::vector<pcl::PointIndices> cluster_indices,
// const sensor_msgs::PointCloud2ConstPtr& pointcloud_msg,
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_transformed,
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud_filtered,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud )
{
// -------------------------------------------------------------------------------------------------------
// Convert image
ROS_INFO_STREAM_NAMED("perception","Converting image to OpenCV format");
try
{
sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
// pcl::toROSMsg (*pointcloud_msg, *image_msg);
pcl::toROSMsg (*cloud_transformed, *image_msg);
cv_bridge::CvImagePtr input_bridge = cv_bridge::toCvCopy(image_msg, "rgb8");
full_input_image = input_bridge->image;
}
catch (cv_bridge::Exception& ex)
{
ROS_ERROR_STREAM_NAMED("perception","[calibrate] Failed to convert image");
return;
}
// -------------------------------------------------------------------------------------------------------
// Process Image
// Convert image to gray
cv::cvtColor( full_input_image, full_input_image_gray, CV_BGR2GRAY );
//cv::adaptiveThreshold( full_input_image, full_input_image_gray, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY,5,10);
// Blur image - reduce noise with a 3x3 kernel
cv::blur( full_input_image_gray, full_input_image_gray, cv::Size(3,3) );
ROS_INFO_STREAM_NAMED("perception","Finished coverting");
// -------------------------------------------------------------------------------------------------------
// Check OpenCV and PCL image height for errors
int image_width = cloud.width;
int image_height = cloud.height;
ROS_DEBUG_STREAM( "PCL Image height " << image_height << " -- width " << image_width << "\n");
int image_width_cv = full_input_image.size.p[1];
int image_height_cv = full_input_image.size.p[0];
ROS_DEBUG_STREAM( "OpenCV Image height " << image_height_cv << " -- width " << image_width_cv << "\n");
if( image_width != image_width_cv || image_height != image_height_cv )
{
ROS_ERROR_STREAM_NAMED("perception","PCL and OpenCV image heights/widths do not match!");
return;
}
// -------------------------------------------------------------------------------------------------------
// GUI Stuff
// First window
const char* opencv_window = "Source";
/*
cv::namedWindow( opencv_window, CV_WINDOW_AUTOSIZE );
cv::imshow( opencv_window, full_input_image_gray );
*/
// while(true) // use this when we want to tweak the image
{
output_image = full_input_image.clone();
// -------------------------------------------------------------------------------------------------------
// Start processing clusters
ROS_INFO_STREAM_NAMED("perception","Finding min/max in x/y axis");
int top_image_overlay_x = 0; // tracks were to copyTo the mini images
// for each cluster, see if it is a block
for(size_t c = 0; c < cluster_indices.size(); ++c)
{
ROS_INFO_STREAM_NAMED("perception","\n\n");
ROS_INFO_STREAM_NAMED("perception","On cluster " << c);
// find the outer dimensions of the cluster
double xmin = 0; double xmax = 0;
double ymin = 0; double ymax = 0;
// also remember each min & max's correponding other coordinate (not needed for z)
double xminy = 0; double xmaxy = 0;
double yminx = 0; double ymaxx = 0;
// also remember their corresponding indice
int xmini = 0; int xmaxi = 0;
int ymini = 0; int ymaxi = 0;
// loop through and find all min/max of x/y
for(size_t i = 0; i < cluster_indices[c].indices.size(); i++)
{
int j = cluster_indices[c].indices[i];
// Get RGB from point cloud
pcl::PointXYZRGB p = cloud_transformed->points[j];
double x = p.x;
double y = p.y;
if(i == 0) // initial values
{
xmin = xmax = x;
ymin = ymax = y;
xminy = xmaxy = y;
yminx = ymaxx = x;
xmini = xmaxi = ymini = ymaxi = j; // record the indice corresponding to the min/max
}
else
{
if( x < xmin )
{
xmin = x;
xminy = y;
xmini = j;
}
if( x > xmax )
{
xmax = x;
xmaxy = y;
xmaxi = j;
}
if( y < ymin )
{
ymin = y;
yminx = x;
ymini = j;
}
if( y > ymax )
{
ymax = y;
ymaxx = x;
ymaxi = j;
}
}
}
ROS_DEBUG_STREAM_NAMED("perception","Cluster size - xmin: " << xmin << " xmax: " << xmax << " ymin: " << ymin << " ymax: " << ymax);
ROS_DEBUG_STREAM_NAMED("perception","Cluster size - xmini: " << xmini << " xmaxi: " << xmaxi << " ymini: " << ymini << " ymaxi: " << ymaxi);
// ---------------------------------------------------------------------------------------------
// Check if these dimensions make sense for the block size specified
double xside = xmax-xmin;
double yside = ymax-ymin;
const double tol = 0.01; // 1 cm error tolerance
// In order to be part of the block, xside and yside must be between
// blocksize and blocksize*sqrt(2)
if(xside > block_size-tol &&
xside < block_size*sqrt(2)+tol &&
yside > block_size-tol &&
yside < block_size*sqrt(2)+tol )
{
// -------------------------------------------------------------------------------------------------------
// Get the four farthest corners of the block - use OpenCV only on the region identified by PCL
// Get the pixel coordinates of the xmax and ymax indicies
int px_xmax = 0; int py_xmax = 0;
int px_ymax = 0; int py_ymax = 0;
getXYCoordinates( xmaxi, image_height, image_width, px_xmax, py_xmax);
getXYCoordinates( ymaxi, image_height, image_width, px_ymax, py_ymax);
// Get the pixel coordinates of the xmin and ymin indicies
int px_xmin = 0; int py_xmin = 0;
int px_ymin = 0; int py_ymin = 0;
getXYCoordinates( xmini, image_height, image_width, px_xmin, py_xmin);
getXYCoordinates( ymini, image_height, image_width, px_ymin, py_ymin);
ROS_DEBUG_STREAM_NAMED("perception","px_xmin " << px_xmin << " px_xmax: " << px_xmax << " py_ymin: " << py_ymin << " py_ymax: " << py_ymax );
// -------------------------------------------------------------------------------------------------------
// Change the frame of reference from the robot to the camera
// Create an array of all the x value options
const int x_values_a[] = {px_xmax, px_ymax, px_xmin, px_ymin};
const int y_values_a[] = {py_xmax, py_ymax, py_xmin, py_ymin};
// Turn it into a vector
std::vector<int> x_values (x_values_a, x_values_a + sizeof(x_values_a) / sizeof(x_values_a[0]));
std::vector<int> y_values (y_values_a, y_values_a + sizeof(y_values_a) / sizeof(y_values_a[0]));
// Find the min
int x1 = *std::min_element(x_values.begin(), x_values.end());
int y1 = *std::min_element(y_values.begin(), y_values.end());
// Find the max
int x2 = *std::max_element(x_values.begin(), x_values.end());
int y2 = *std::max_element(y_values.begin(), y_values.end());
ROS_DEBUG_STREAM_NAMED("perception","x1: " << x1 << " y1: " << y1 << " x2: " << x2 << " y2: " << y2);
// -------------------------------------------------------------------------------------------------------
// Expand the ROI by a fudge factor, if possible
const int FUDGE_FACTOR = 5; // pixels
if( x1 > FUDGE_FACTOR)
x1 -= FUDGE_FACTOR;
if( y1 > FUDGE_FACTOR )
y1 -= FUDGE_FACTOR;
if( x2 < image_width - FUDGE_FACTOR )
x2 += FUDGE_FACTOR;
if( y2 < image_height - FUDGE_FACTOR )
y2 += FUDGE_FACTOR;
ROS_DEBUG_STREAM_NAMED("perception","After Fudge Factor - x1: " << x1 << " y1: " << y1 << " x2: " << x2 << " y2: " << y2);
// -------------------------------------------------------------------------------------------------------
// Create ROI parameters
// (x1,y1)----------------------
// | |
// | ROI |
// | |
// |_____________________(x2,y2)|
// Create Region of Interest
int roi_width = x2 - x1;
int roi_height = y2 - y1;
cv::Rect region_of_interest = cv::Rect( x1, y1, roi_width, roi_height );
ROS_DEBUG_STREAM_NAMED("perception","ROI: x " << x1 << " -- y " << y1 << " -- height " << roi_height << " -- width " << roi_width );
// -------------------------------------------------------------------------------------------------------
// Find paramters of the block in pixel coordiantes
int block_center_x = x1 + 0.5*roi_width;
int block_center_y = y1 + 0.5*roi_height;
int block_center_z = block_size / 2; // TODO: make this better
const cv::Point block_center = cv::Point( block_center_x, block_center_y );
// -------------------------------------------------------------------------------------------------------
// Create a sub image of just the block
cv::Point a1 = cv::Point(x1, y1);
cv::Point a2 = cv::Point(x2, y2);
cv::rectangle( output_image, a1, a2, cv::Scalar(0, 255, 255), 1, 8);
// Crop image (doesn't actually copy the data)
cropped_image = full_input_image_gray(region_of_interest);
// -------------------------------------------------------------------------------------------------------
// Detect edges using canny
ROS_INFO_STREAM_NAMED("perception","Detecting edges using canny");
// Find edges
cv::Mat canny_output;
cv::Canny( cropped_image, canny_output, canny_threshold, canny_threshold*2, 3 );
// Get mini window stats
const int mini_width = canny_output.size.p[1];
const int mini_height = canny_output.size.p[0];
const cv::Size mini_size = canny_output.size();
const cv::Point mini_center = cv::Point( mini_width/2, mini_height/2 );
// Find contours
vector<vector<cv::Point> > contours;
vector<cv::Vec4i> hierarchy;
cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
ROS_INFO_STREAM_NAMED("perception","Contours");
// Draw contours
cv::Mat drawing = cv::Mat::zeros( mini_size, CV_8UC3 );
ROS_INFO_STREAM_NAMED("perception","Drawing contours");
// Find the largest contour for getting the angle
double max_contour_length = 0;
int max_contour_length_i;
for( size_t i = 0; i< contours.size(); i++ )
{
double contour_length = cv::arcLength( contours[i], false );
if( contour_length > max_contour_length )
{
max_contour_length = contour_length;
max_contour_length_i = i;
}
//ROS_DEBUG_STREAM_NAMED("perception","Contour length = " << contour_length << " of index " << max_contour_length_i);
cv::Scalar color = cv::Scalar( (30 + i*10) % 255, (30 + i*10) % 255, (30 + i*10) % 255);
cv::drawContours( drawing, contours, (int)i, color, 1, 8, hierarchy, 0, cv::Point() );
//drawContours( image, contours, contourIdx, color, thickness, lineType, hierarchy, maxLevel, offset )
}
// -------------------------------------------------------------------------------------------------------
// Copy largest contour to main image
cv::Scalar color = cv::Scalar( 0, 255, 0 );
cv::drawContours( output_image, contours, (int)max_contour_length_i, color, 1, 8, hierarchy, 0, a1 );
//drawContours( image, contours, contourIdx, color, thickness, lineType, hierarchy, maxLevel, offset )
// -------------------------------------------------------------------------------------------------------
// Copy largest contour to seperate image
cv::Mat hough_input = cv::Mat::zeros( mini_size, CV_8UC1 );
cv::Mat hough_input_color;
cv::Scalar hough_color = cv::Scalar( 200 );
cv::drawContours( hough_input, contours, (int)max_contour_length_i, hough_color, 1, 8, hierarchy, 0 );
cv::cvtColor(hough_input, hough_input_color, CV_GRAY2BGR);
// -------------------------------------------------------------------------------------------------------
// Hough Transform
cv::Mat hough_drawing = cv::Mat::zeros( mini_size, CV_8UC3 );
std::vector<cv::Vec4i> lines;
ROS_DEBUG_STREAM_NAMED("perception","hough_rho " << hough_rho << " hough_theta " << hough_theta <<
" theta_converted " << (1/hough_theta)*CV_PI/180 << " hough_threshold " <<
hough_threshold << " hough_minLineLength " << hough_minLineLength <<
" hough_maxLineGap " << hough_maxLineGap );
cv::HoughLinesP(hough_input, lines, hough_rho, (1/hough_theta)*CV_PI/180, hough_threshold, hough_minLineLength, hough_maxLineGap);
ROS_WARN_STREAM_NAMED("perception","Found " << lines.size() << " lines");
std::vector<double> line_angles;
// Copy detected lines to the drawing image
for( size_t i = 0; i < lines.size(); i++ )
{
cv::Vec4i line = lines[i];
cv::line( hough_drawing, cv::Point(line[0], line[1]), cv::Point(line[2], line[3]),
cv::Scalar(255,255,255), 1, CV_AA);
// Error check
if(line[3] - line[1] == 0 && line[2] - line[0] == 0)
{
ROS_ERROR_STREAM_NAMED("perception","Line is actually two points at the origin, unable to calculate. TODO: handle better?");
continue;
}
// Find angle
double line_angle = atan2(line[3] - line[1], line[2] - line[0]); //in radian, degrees: * 180.0 / CV_PI;
// Reverse angle direction if negative
if( line_angle < 0 )
{
line_angle += CV_PI;
}
line_angles.push_back(line_angle);
ROS_DEBUG_STREAM_NAMED("perception","Hough Line angle: " << line_angle * 180.0 / CV_PI;);
}
double block_angle = 0; // the overall result of the block's angle
// Everything is based on the first angle
if( line_angles.size() == 0 ) // make sure we have at least 1 angle
{
ROS_ERROR_STREAM_NAMED("perception","No lines were found for this cluster, unable to calculate block angle");
}
else
{
calculateBlockAngle( line_angles, block_angle );
}
// -------------------------------------------------------------------------------------------------------
// Draw chosen angle
ROS_INFO_STREAM_NAMED("perception","Using block angle " << block_angle*180.0/CV_PI);
// Draw chosen angle on mini image
cv::Mat angle_drawing = cv::Mat::zeros( mini_size, CV_8UC3 );
int line_length = 0.5*double(mini_width); // have the line go 1/4 across the screen
int new_x = mini_center.x + line_length*cos( block_angle );
int new_y = mini_center.y + line_length*sin( block_angle );
ROS_INFO_STREAM("Origin (" << mini_center.x << "," << mini_center.y << ") New (" << new_x << "," << new_y <<
") length " << line_length << " angle " << block_angle <<
" mini width " << mini_width << " mini height " << mini_height);
cv::Point angle_point = cv::Point(new_x, new_y);
cv::line( angle_drawing, mini_center, angle_point, cv::Scalar(255,255,255), 1, CV_AA);
// Draw chosen angle on contours image
cv::line( hough_drawing, mini_center, angle_point, cv::Scalar(255,0, 255), 1, CV_AA);
// Draw chosen angle on main image
line_length = 0.75 * double(mini_width); // have the line go 1/2 across the box
new_x = block_center_x + line_length*cos( block_angle );
new_y = block_center_y + line_length*sin( block_angle );
ROS_INFO_STREAM_NAMED("perception",block_center_x << ", " << block_center_y << ", " << new_x << ", " << new_y);
angle_point = cv::Point(new_x, new_y);
cv::line( output_image, block_center, angle_point, cv::Scalar(255,0,255), 2, CV_AA);
// -------------------------------------------------------------------------------------------------------
// Get world coordinates
// Find the block's center point
double world_x1 = xmin+(xside)/2.0;
double world_y1 = ymin+(yside)/2.0;
double world_z1 = table_height + block_size / 2;
// Convert pixel coordiantes back to world coordinates
double world_x2 = cloud_transformed->at(new_x, new_y).x;
double world_y2 = cloud_transformed->at(new_x, new_y).y;
double world_z2 = world_z1; // is this even necessary?
// Get angle from two world coordinates...
double world_theta = abs( atan2(world_y2 - world_y1, world_x2 - world_x1) );
// Attempt to make all angles point in same direction
makeAnglesUniform( world_theta );
// -------------------------------------------------------------------------------------------------------
// GUI Stuff
// Copy the cluster image to the main image in the top left corner
if( top_image_overlay_x + mini_width < image_width )
{
const int common_height = 42;
cv::Rect small_roi_row0 = cv::Rect(top_image_overlay_x, common_height*0, mini_width, mini_height);
cv::Rect small_roi_row1 = cv::Rect(top_image_overlay_x, common_height*1, mini_width, mini_height);
cv::Rect small_roi_row2 = cv::Rect(top_image_overlay_x, common_height*2, mini_width, mini_height);
cv::Rect small_roi_row3 = cv::Rect(top_image_overlay_x, common_height*3, mini_width, mini_height);
drawing.copyTo( output_image(small_roi_row0) );
hough_input_color.copyTo( output_image(small_roi_row1) );
hough_drawing.copyTo( output_image(small_roi_row2) );
angle_drawing.copyTo( output_image(small_roi_row3) );
top_image_overlay_x += mini_width;
}
// figure out the position and the orientation of the block
//double angle = atan(block_size/((xside+yside)/2));
//double angle = atan( (xmaxy - xminy) / (xmax - xmin ) );
// Then add it to our set
//addBlock( xmin+(xside)/2.0, ymin+(yside)/2.0, zmax - block_size/2.0, angle);
//ROS_INFO_STREAM_NAMED("perception","FOUND -> xside: " << xside << " yside: " << yside << " angle: " << block_angle);
addBlock( world_x1, world_y1, world_z1, world_theta );
//addBlock( block_center_x, block_center_y, block_center_z, block_angle);
}
else
{
void DeterministicGameState::predictGhostMove(int ghost_index)
{
ROS_DEBUG_STREAM("Predict ghost " << ghost_index);
// TODO: add things here
}
int getStaticTransform(string calib_filename, geometry_msgs::TransformStamped *ros_msgTf, std_msgs::Header *header, string camera_name)
{
ifstream calib_file(calib_filename.c_str());
if (!calib_file.is_open())
return false;
ROS_DEBUG_STREAM("Reading transformation" << (camera_name.empty()? string(""): string(" for camera ")+camera_name) << " from " << calib_filename);
typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
boost::char_separator<char> sep{" "};
string line="";
char index=0;
tokenizer::iterator token_iterator;
tf2::Transform t;
while (getline(calib_file,line))
{
// Parse string phase 1, tokenize it using Boost.
tokenizer tok(line,sep);
// Move the iterator at the beginning of the tokenize vector and check for T/R matrices.
token_iterator=tok.begin();
if (strcmp((*token_iterator).c_str(),((string)(string("T")+(camera_name.empty()?string(""):string("_")+camera_name)+string(":"))).c_str())==0) //Calibration Matrix
{
index=0; //should be 3 at the end
ROS_DEBUG_STREAM("T" << (camera_name.empty()?string(""):string("_")+camera_name));
double T[3];
for (token_iterator++; token_iterator != tok.end(); token_iterator++)
{
// std::cout << *token_iterator << '\n';
T[index++]=boost::lexical_cast<double>(*token_iterator);
}
t.setOrigin(tf2::Vector3(T[0],T[1],T[2]));
}
// EXPERIMENTAL: use with unrectified images
// token_iterator=tok.begin();
// if (strcmp((*token_iterator).c_str(),((string)(string("D_")+camera_name+string(":"))).c_str())==0) //Distortion Coefficients
// {
// index=0; //should be 5 at the end
// ROS_DEBUG_STREAM("D_" << camera_name);
// for (token_iterator++; token_iterator != tok.end(); token_iterator++)
// {
//// std::cout << *token_iterator << '\n';
// D[index++]=boost::lexical_cast<double>(*token_iterator);
// }
// }
token_iterator=tok.begin();
if (strcmp((*token_iterator).c_str(),((string)(string("R")+(camera_name.empty()?string(""):string("_")+camera_name)+string(":"))).c_str())==0) //Rectification Matrix
{
index=0; //should be 9 at the end
ROS_DEBUG_STREAM("R" << (camera_name.empty()?string(""):string("_")+camera_name));
double R[9];
for (token_iterator++; token_iterator != tok.end(); token_iterator++)
{
// std::cout << *token_iterator << '\n';
R[index++]=boost::lexical_cast<double>(*token_iterator);
}
tf2::Matrix3x3 mat(R[0],R[1],R[2],
R[3],R[4],R[5],
R[6],R[7],R[8]);
tf2::Quaternion quat; mat.getRotation(quat);
t.setRotation(quat);
}
// token_iterator=tok.begin();
// if (strcmp((*token_iterator).c_str(),((string)(string("P_rect_")+camera_name+string(":"))).c_str())==0) //Projection Matrix Rectified
// {
// index=0; //should be 12 at the end
// ROS_DEBUG_STREAM("P_rect_" << camera_name);
// for (token_iterator++; token_iterator != tok.end(); token_iterator++)
// {
// //std::cout << *token_iterator << '\n';
// P[index++]=boost::lexical_cast<double>(*token_iterator);
// }
// }
}
// fill up tf message
ros_msgTf->header.frame_id = header->frame_id;
ros_msgTf->header.stamp = header->stamp;
// Invert transform to match the tf tree structure: oxts -> velodyne -> cam00 -> cam{01,02,03}
tf2::convert(t.inverse(),ros_msgTf->transform);
ROS_DEBUG_STREAM("... ok");
return true;
}
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);
collision_detection::CollisionRequest creq;
creq.group_name = req.group_name;
collision_detection::CollisionResult cres;
planning_scene->checkCollision(creq, cres, start_state);
if (cres.collision)
{
// Rerun in verbose mode
collision_detection::CollisionRequest vcreq = creq;
collision_detection::CollisionResult vcres;
vcreq.verbose = true;
planning_scene->checkCollision(vcreq, vcres, start_state);
if (creq.group_name.empty())
ROS_INFO("Start state appears to be in collision");
else
ROS_INFO_STREAM("Start state appears to be in collision with respect to group " << creq.group_name);
robot_state::RobotStatePtr prefix_state(new robot_state::RobotState(start_state));
random_numbers::RandomNumberGenerator &rng = prefix_state->getRandomNumberGenerator();
const std::vector<const robot_model::JointModel*> &jmodels =
planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ?
planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() :
planning_scene->getRobotModel()->getJointModels();
bool found = false;
for (int c = 0 ; !found && c < sampling_attempts_ ; ++c)
{
for (std::size_t i = 0 ; !found && i < jmodels.size() ; ++i)
{
std::vector<double> sampled_variable_values(jmodels[i]->getVariableCount());
const double *original_values = prefix_state->getJointPositions(jmodels[i]);
jmodels[i]->getVariableRandomPositionsNearBy(rng, &sampled_variable_values[0], original_values, jmodels[i]->getMaximumExtent() * jiggle_fraction_);
start_state.setJointPositions(jmodels[i], sampled_variable_values);
collision_detection::CollisionResult cres;
planning_scene->checkCollision(creq, cres, start_state);
if (!cres.collision)
{
found = true;
ROS_INFO("Found a valid state near the start state at distance %lf after %d attempts", prefix_state->distance(start_state), c);
}
}
}
if (found)
{
planning_interface::MotionPlanRequest req2 = req;
robot_state::robotStateToRobotStateMsg(start_state, req2.start_state);
bool solved = planner(planning_scene, req2, res);
if (solved && !res.trajectory_->empty())
{
// heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to the computed trajectory)
res.trajectory_->setWayPointDurationFromPrevious(0, std::min(max_dt_offset_, res.trajectory_->getAverageSegmentDuration()));
res.trajectory_->addPrefixWayPoint(prefix_state, 0.0);
// we add a prefix point, so we need to bump any previously added index positions
for (std::size_t i = 0 ; i < added_path_index.size() ; ++i)
added_path_index[i]++;
added_path_index.push_back(0);
}
return solved;
}
else
{
ROS_WARN("Unable to find a valid state nearby the start state (using jiggle fraction of %lf and %u sampling attempts). Passing the original planning request to the planner.",
jiggle_fraction_, sampling_attempts_);
return planner(planning_scene, req, res);
}
}
else
{
if (creq.group_name.empty())
ROS_DEBUG("Start state is valid");
else
ROS_DEBUG_STREAM("Start state is valid with respect to group " << creq.group_name);
return planner(planning_scene, req, res);
}
}
