void initialize(UAS &uas_)
{
double conn_system_time_d;
double conn_timesync_d;
ros::Duration conn_system_time;
ros::Duration conn_timesync;
uas = &uas_;
if (nh.getParam("conn/system_time_rate", conn_system_time_d) && conn_system_time_d != 0.0) {
conn_system_time = ros::Duration(ros::Rate(conn_system_time_d));
}
else if (nh.getParam("conn/system_time", conn_system_time_d)) {
// XXX deprecated parameter
ROS_WARN_NAMED("time", "TM: parameter `~conn/system_time` deprecated, "
"please use `~conn/system_time_rate` instead!");
conn_system_time = ros::Duration(conn_system_time_d);
}
if (nh.getParam("conn/timesync_rate", conn_timesync_d) && conn_timesync_d != 0.0) {
conn_timesync = ros::Duration(ros::Rate(conn_timesync_d));
}
else if (nh.getParam("conn/timesync", conn_timesync_d)) {
// XXX deprecated parameter
ROS_WARN_NAMED("time", "TM: parameter `~conn/timesync` deprecated, "
"please use `~conn/timesync_rate` instead!");
conn_timesync = ros::Duration(conn_timesync_d);
}
nh.param<std::string>("time/time_ref_source", time_ref_source, "fcu");
nh.param("time/timesync_avg_alpha", offset_avg_alpha, 0.6);
/*
* alpha for exponential moving average. The closer alpha is to 1.0,
* the faster the moving average updates in response to new offset samples (more jitter)
* We need a significant amount of smoothing , more so for lower message rates like 1Hz
*/
time_ref_pub = nh.advertise<sensor_msgs::TimeReference>("time_reference", 10);
// timer for sending system time messages
if (!conn_system_time.isZero()) {
sys_time_timer = nh.createTimer(conn_system_time,
&SystemTimePlugin::sys_time_cb, this);
sys_time_timer.start();
}
// timer for sending timesync messages
if (!conn_timesync.isZero()) {
// enable timesync diag only if that feature enabled
UAS_DIAG(uas).add(dt_diag);
timesync_timer = nh.createTimer(conn_timesync,
&SystemTimePlugin::timesync_cb, this);
timesync_timer.start();
}
}
MovePlatformAction() :
as_(n_, CARLOS_MOVE_ACTION, false), //movePlatform action SERVER
ac_planner_("/plan_action", true), // Planner action CLIENT
ac_control_("/control_action", true) // Controller action CLIENT
{
n_.param("/move_platform_server/debug", debug_, false);
std::string name = ROSCONSOLE_DEFAULT_NAME; //ros.carlos_motion_action_server
name = name + ".debug";
logger_name_ = "debug";
//logger is ros.carlos_motion_action_server.debug
if (debug_)
{
// if we use ROSCONSOLE_DEFAULT_NAME we'll get a ton of debug messages from actionlib which is annoying!!!
// so for debug we'll use a named logger
if(ros::console::set_logger_level(name, ros::console::levels::Debug)) //name
ros::console::notifyLoggerLevelsChanged();
}
else // if not DEBUG we want INFO
{
if(ros::console::set_logger_level(name, ros::console::levels::Info)) //name
ros::console::notifyLoggerLevelsChanged();
}
ROS_DEBUG_NAMED(logger_name_, "Starting Move Platform Server");
as_.registerGoalCallback(boost::bind(&MovePlatformAction::moveGoalCB, this));
as_.registerPreemptCallback(boost::bind(&MovePlatformAction::movePreemptCB, this));
//start the move server
as_.start();
ROS_DEBUG_NAMED(logger_name_, "Move Platform Action Server Started");
// now wait for the other servers (planner + controller) to start
ROS_WARN_NAMED(logger_name_, "Waiting for planner server to start");
ac_planner_.waitForServer();
ROS_INFO_STREAM_NAMED(logger_name_, "Planner server started: " << ac_planner_.isServerConnected());
ROS_WARN_NAMED(logger_name_, "Waiting for controller server to start");
ac_control_.waitForServer();
ROS_INFO_STREAM_NAMED(logger_name_, "Controller server started: " << ac_control_.isServerConnected());
n_.param("/carlos/fsm_frequency", frequency_, DEFAULT_STATE_FREQ);
state_pub_timer_ = n_.createTimer(frequency_, &MovePlatformAction::state_pub_timerCB, this);
state_pub_ = n_.advertise<mission_ctrl_msgs::hardware_state>(CARLOS_BASE_STATE_MSG,1);
planning_ = false;
controlling_ = false;
//set_terminal_state_;
ctrl_state_sub = n_.subscribe<std_msgs::String>("/oea_controller/controller_state", 5, &MovePlatformAction::control_stateCB, this);
planner_state_sub = n_.subscribe<std_msgs::UInt8>("/oea_planner/state", 5, &MovePlatformAction::planner_stateCB, this);
}
bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) {
// dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal
if ( ! costmap_ros_->getRobotPose(current_pose_)) {
ROS_ERROR("Could not get robot pose");
return false;
}
std::vector<geometry_msgs::PoseStamped> transformed_plan;
if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) {
ROS_ERROR("Could not get local plan");
return false;
}
//if the global plan passed in is empty... we won't do anything
if(transformed_plan.empty()) {
ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan.");
return false;
}
ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());
//we want to clear the robot footprint from the costmap we're using
costmap_ros_->clearRobotFootprint();
// make sure to update the costmap we'll use for this cycle
costmap_ros_->getCostmapCopy(costmap_);
// update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory
dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan);
if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) {
//publish an empty plan because we've reached our goal position
std::vector<geometry_msgs::PoseStamped> local_plan;
std::vector<geometry_msgs::PoseStamped> transformed_plan;
publishGlobalPlan(transformed_plan);
publishLocalPlan(local_plan);
base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits();
return latchedStopRotateController_.computeVelocityCommandsStopRotate(
cmd_vel,
limits.getAccLimits(),
dp_->getSimPeriod(),
&planner_util_,
odom_helper_,
current_pose_,
boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3));
} else {
bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel);
if (isOk) {
publishGlobalPlan(transformed_plan);
} else {
ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");
std::vector<geometry_msgs::PoseStamped> empty_plan;
publishGlobalPlan(empty_plan);
}
return isOk;
}
}
inline bool is_normalized(float throttle, const float min, const float max) {
if (throttle < min) {
ROS_WARN_NAMED("attitude", "Not normalized throttle! Thd(%f) < Min(%f)", throttle, min);
return false;
}
else if (throttle > max) {
ROS_WARN_NAMED("attitude", "Not normalized throttle! Thd(%f) > Max(%f)", throttle, max);
return false;
}
return true;
}
// If robot has e--top button pressed, print a warning and return true. Otherwise return false.
bool check_estop(const char *s) {
arnl.robot->lock();
bool e = arnl.robot->isEStopPressed();
arnl.robot->unlock();
if(e) ROS_WARN_NAMED("rosarnl_node", "rosarnl_node: Warning: Robot e-stop button pressed, cannot %s", s);
return e;
}
void write()
{
if(!initted)
{
ROS_WARN_NAMED(APPLICATION_NAME, "hw interface was not initted yet (not able to connect to the robot), cannot send command to the robot\n");
return;
}
// write only meaningful values, if possible.
printf("out: - size of controlledJoints is %d\n", (int) controlledJoints.size());
if(controlledJoints.size() == 0)
return;
msg_clear();
for(int i=0; i<controlledJoints.size(); i++)
{
output_msg.names.push_back(controlledJoints[i].first);
output_msg.referenceType.push_back(controlledJoints[i].second);
output_msg.reference.push_back(outCmd[jointMap.at(controlledJoints[i].first)]);
printf("[%2d - %s] mode: %d; cmd: %f [%f]\n", i, controlledJoints[i].first.c_str(), output_msg.referenceType[i], output_msg.reference[i], outCmd[i]);
}
printf("\n"); fflush(stdout);
yarpRosCmd_publisher.publish(output_msg);
}
void autopilot_version_cb(const ros::TimerEvent &event) {
bool ret = false;
try {
auto client = nh.serviceClient<mavros::CommandLong>("cmd/command");
mavros::CommandLong cmd{};
cmd.request.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
cmd.request.confirmation = false;
cmd.request.param1 = 1.0;
ROS_DEBUG_NAMED("sys", "VER: Sending request.");
ret = client.call(cmd);
}
catch (ros::InvalidNameException &ex) {
ROS_ERROR_NAMED("sys", "VER: %s", ex.what());
}
ROS_ERROR_COND_NAMED(!ret, "sys", "VER: command plugin service call failed!");
if (version_retries > 0) {
version_retries--;
ROS_WARN_COND_NAMED(version_retries != RETRIES_COUNT - 1, "sys",
"VER: request timeout, retries left %d", version_retries);
}
else {
uas->update_capabilities(false);
autopilot_version_timer.stop();
ROS_WARN_NAMED("sys", "VER: your FCU don't support AUTOPILOT_VERSION, "
"switched to default capabilities");
}
}
DoorTraj::~DoorTraj() {
last_traj_id = id;
if (pthread_create(&recorder_thread,NULL,&write_recorder_data,(void*)recorder)) {
ROS_WARN_NAMED("OpenDoor","Could not create thread to write log data");
}
hybridplug->current_traj=NULL;
}
void ControlDoneCB(const actionlib::SimpleClientGoalState& state, const oea_controller::controlPlatformResultConstPtr &result)
{
controlling_ = false;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Control Action finished: " << state.toString());
move_result_.result_state = result->result_state;
move_result_.error_string = result->error_string;
if (move_result_.result_state)
{
as_.setSucceeded(move_result_);
ROS_INFO_NAMED(logger_name_, "Goal was successful :)");
}
else
{
ROS_WARN_NAMED(logger_name_, "Goal was NOT successful :)");
// if is preempted => as_ was already set, cannot set again
if (state.toString() != "PREEMPTED")
{
as_.setAborted(move_result_);
ROS_DEBUG_NAMED(logger_name_, "Goal was Aborted");
}
else
{
if (set_terminal_state_)
{
as_.setPreempted(move_result_);
ROS_DEBUG_NAMED(logger_name_, "Goal was Preempted");
}
}
}
}
/*! \brief Sets planning scene based on the stored list of objects and allowed contact region and returns the scene.*/
arm_navigation_msgs::SetPlanningSceneDiff::Response collisionObjects::setPlanningScene(){
arm_navigation_msgs::SetPlanningSceneDiff::Request req;
arm_navigation_msgs::SetPlanningSceneDiff::Response res;
for(unsigned int i=0; i<_att_obj.size(); i++){
if(!_att_obj[i].object.shapes.empty()){
req.planning_scene_diff.attached_collision_objects.push_back(_att_obj[i]);
}
}
for(unsigned int i=0; i<_coll_obj.size(); i++){
if(!_coll_obj[i].shapes.empty()){
req.planning_scene_diff.collision_objects.push_back(_coll_obj[i]);
}
}
if(!_allowed_contact.shape.dimensions.empty()){
req.planning_scene_diff.allowed_contacts.push_back(_allowed_contact);
}
if(!_set_pln_scn_client.call(req, res)){
ROS_ERROR_NAMED(CL_LGRNM,"Call to %s failed", SET_PLANNING_SCENE_DIFF_NAME);
if(!ros::service::exists(SET_PLANNING_SCENE_DIFF_NAME, true)){
ROS_WARN_NAMED(CL_LGRNM,"%s service doesn't exist", SET_PLANNING_SCENE_DIFF_NAME);
}
}
//printListOfObjects();
return res;
}
bool stop(Stop::Request &req, Stop::Response &res){
KukaRequest kuka_req( 10, STOP); //(priority, SRV)
addRequest(kuka_req);
ROS_WARN_NAMED(name_, "Stop command received.");
res.result = true;
return true;
}
bool RosArnlNode::global_localization_srv_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Localize init (global_localization service) request...");
// arnl.locTask->localizeRobotInMapInit();
if(! arnl.locTask->localizeRobotAtHomeBlocking() )
ROS_WARN_NAMED("rosarnl_node", "rosarnl_node: Error in initial localization.");
return true;
}
void Navigation::doneCb(const actionlib::SimpleClientGoalState& state,
const move_base_msgs::MoveBaseResultConstPtr& result)
{
if( move_base_ac_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_WARN_NAMED(name_, "Move base failed to approach target");
move_base_ac_.cancelAllGoals();
move_base_msgs::MoveBaseGoal goal_msg;
ROS_INFO_NAMED(name_,"GOING TO recovery pose: %i", BOX_CHARGE);
goal_msg.target_pose.pose = recovery_;
sendMoveBaseGoal(goal_msg);
in_recovery_mode = true;
//result_.state = free_navigation::NavigateFreelyResult::FAILED;
//as_.setAborted(result_,"Move base failed to approach target");
return;
}
// if at staging area then approach previous goal again
if(in_recovery_mode)
{
in_recovery_mode = false;
approachGoal();
}
else
{
//ROS_INFO_NAMED(name_,"Finished in state: %s", state.getText().c_str());
switch (goal_) {
case CHARGE:
ROS_INFO_NAMED(name_,"Docking in CHARGER: %i", BOX_CHARGE);
{
docking_with_walls::docking_with_wallsGoal dock_goal;
docking_with_walls_ac_.sendGoal(dock_goal, boost::bind(&Navigation::doneCbWalls,this,_1,_2));
}
break;
case BRICK:
ROS_INFO_NAMED(name_,"going in to collect bricks: %i", BRICK);
{
collect_bricks_pos::collect_bricks_posGoal collect_bricks_goal;
collect_bricks_ac_.sendGoal(collect_bricks_goal, boost::bind(&Navigation::doneCollectBricksCv,this,_1,_2));
}
break;
case DELIVERY:
ROS_INFO_NAMED(name_,"Tipping of at DELIVERY: %i", DELIVERY);
ros::Duration(2.0).sleep();
as_.setSucceeded(result_);
current_position = Navigation::free;
break;
case TRANSITION:
ROS_INFO_NAMED(name_,"At transition: %i", TRANSITION);
as_.setSucceeded(result_);
current_position = Navigation::at_transition;
break;
default:
//ac_.cancelAllGoals();
return; // not a navigation command so do not navigate
}
}
}
/**
* @brief Function to verify if the thrust values are normalized;
* considers also the reversed trust values
*/
inline bool is_normalized(float thrust){
if (reverse_thrust) {
if (thrust < -1.0) {
ROS_WARN_NAMED("attitude", "Not normalized reversed thrust! Thd(%f) < Min(%f)", thrust, -1.0);
return false;
}
}
else {
if (thrust < 0.0) {
ROS_WARN_NAMED("attitude", "Not normalized thrust! Thd(%f) < Min(%f)", thrust, 0.0);
return false;
}
}
if (thrust > 1.0) {
ROS_WARN_NAMED("attitude", "Not normalized thrust! Thd(%f) > Max(%f)", thrust, 1.0);
return false;
}
return true;
}
void QuadrotorPropulsion::addPWMToQueue(const hector_uav_msgs::MotorPWMConstPtr& pwm)
{
boost::mutex::scoped_lock lock(command_queue_mutex_);
if (!motor_status_.on) {
ROS_WARN_NAMED("quadrotor_propulsion", "Received new motor command. Enabled motors.");
engage();
}
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Received motor command valid at " << pwm->header.stamp);
command_queue_.push(pwm);
command_condition_.notify_all();
}
void ompl_interface::ModelBasedStateSpace::setTagSnapToSegment(double snap)
{
if (snap < 0.0 || snap > 1.0)
ROS_WARN_NAMED("model_based_state_space",
"Snap to segment for tags is a ratio. It's value must be between 0.0 and 1.0. "
"Value remains as previously set (%lf)",
tag_snap_to_segment_);
else
{
tag_snap_to_segment_ = snap;
tag_snap_to_segment_complement_ = 1.0 - tag_snap_to_segment_;
}
}
bool read()
{
printf("In:\n");
int msgLenght;
if(initted)
{
mutex.lock();
msgLenght = state_msg->name.size();
if(msgLenght <= 0)
{
ROS_ERROR_NAMED(APPLICATION_NAME, "Received malformed message of size %d", msgLenght);
return false;
}
// size of msg can be smaller than number of joints, but not bigger
if(msgLenght > joint_num)
{
ROS_ERROR_NAMED(APPLICATION_NAME, "Size of input data is %d, which is bigger than numer of joints [%d]", msgLenght, joint_num);
return false;
}
if(state_msg->position.size() != msgLenght)
{
ROS_ERROR_NAMED(APPLICATION_NAME, "Received malformed message. Size of 'names' is different from size of references.");
return false;
}
// same check for other fields?? TBD: define this f*****g msg!!!
// Joints can be in any order, so we need a map to fill in the right spot.
for(int i=0; i<msgLenght; i++)
{
// TODO: using '[]' operator may cause issues, use '.at()' instead
measPos[jointMap[state_msg->name[i]]] = state_msg->position[i];
measVel[jointMap[state_msg->name[i]]] = state_msg->velocity[i];
measEff[jointMap[state_msg->name[i]]] = state_msg->effort[i];
printf("[%2d] %-*s %8.4f\t%8.4f\t%8.4f\n", i, longestJointName_size+2, state_msg->name[i].c_str(), measPos[i], measVel[i], measEff[i]);
}
ROS_WARN_NAMED(APPLICATION_NAME, "TBS: Try to get the most recent reference target maybe??\n");
mutex.unlock();
fflush(stdout);
}
else
{
std::cout << "Not initted yet" << std::endl;
}
return true;
}
void checkImagesSynchronized()
{
int threshold = 3 * both_received_;
if (image_received_ > threshold || info_received_ > threshold) {
ROS_WARN_NAMED("sync", // Can suppress ros.image_transport.sync independent of anything else
"[image_transport] Topics '%s' and '%s' do not appear to be synchronized. "
"In the last 10s:\n"
"\tImage messages received: %d\n"
"\tCameraInfo messages received: %d\n"
"\tSynchronized pairs: %d",
image_sub_.getTopic().c_str(), info_sub_.getTopic().c_str(),
image_received_, info_received_, both_received_);
}
image_received_ = info_received_ = both_received_ = 0;
}
void convert_to_bus(SL_Bus_etasetgett_std_msgs_MultiArrayLayout* busPtr, std_msgs::MultiArrayLayout const* msgPtr)
{
busPtr->DataOffset = msgPtr->data_offset;
{
int numItemsToCopy = msgPtr->dim.size();
if (numItemsToCopy > 16)
{
ROS_WARN_NAMED("etasetgett", "Truncating array '%s' in received message '%s' from %d to %d items", "dim", "std_msgs/MultiArrayLayout", numItemsToCopy, 16);
numItemsToCopy = 16;
}
busPtr->Dim_SL_Info.CurrentLength = static_cast<uint32_T>( numItemsToCopy );
for (int i=0; i < numItemsToCopy; i++)
{
convert_to_bus(&busPtr->Dim[i], &msgPtr->dim[i]);
}
}
}
void ariaLogHandler(const char *msg, ArLog::LogLevel level)
{
// node that ARIA logging is normally limited at Normal and Terse only. Set
// ARLOG_LEVEL environment variable to override.
switch(level)
{
case ArLog::Normal:
ROS_INFO_NAMED("ARNL", "ARNL: %s", msg);
return;
case ArLog::Terse:
ROS_WARN_NAMED("ARNL", "ARNL: %s", msg);
return;
case ArLog::Verbose:
ROS_DEBUG_NAMED("ARNL", "ARNL: %s", msg);
return;
}
}
void convert_to_bus(SL_Bus_etasetgett_std_msgs_Float32MultiArray* busPtr, std_msgs::Float32MultiArray const* msgPtr)
{
{
int numItemsToCopy = msgPtr->data.size();
if (numItemsToCopy > 256)
{
ROS_WARN_NAMED("etasetgett", "Truncating array '%s' in received message '%s' from %d to %d items", "data", "std_msgs/Float32MultiArray", numItemsToCopy, 256);
numItemsToCopy = 256;
}
busPtr->Data_SL_Info.CurrentLength = static_cast<uint32_T>( numItemsToCopy );
for (int i=0; i < numItemsToCopy; i++)
{
busPtr->Data[i] = msgPtr->data[i];
}
}
convert_to_bus(&busPtr->Layout, &msgPtr->layout);
}
void move_group::MoveGroupPickPlaceAction::executePickupCallback(const moveit_msgs::PickupGoalConstPtr& input_goal)
{
setPickupState(PLANNING);
// before we start planning, ensure that we have the latest robot state received...
context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
context_->planning_scene_monitor_->updateFrameTransforms();
moveit_msgs::PickupGoalConstPtr goal;
if (input_goal->possible_grasps.empty())
{
moveit_msgs::PickupGoal* copy(new moveit_msgs::PickupGoal(*input_goal));
goal.reset(copy);
fillGrasps(*copy);
}
else
goal = input_goal;
moveit_msgs::PickupResult action_res;
if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
{
if (!goal->planning_options.plan_only)
ROS_WARN_NAMED("manipulation", "This instance of MoveGroup is not allowed to execute trajectories but the pick "
"goal request has plan_only set to false. Only a motion plan will be computed "
"anyway.");
executePickupCallbackPlanOnly(goal, action_res);
}
else
executePickupCallbackPlanAndExecute(goal, action_res);
bool planned_trajectory_empty = action_res.trajectory_stages.empty();
std::string response =
getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only);
if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
pickup_action_server_->setSucceeded(action_res, response);
else
{
if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
pickup_action_server_->setPreempted(action_res, response);
else
pickup_action_server_->setAborted(action_res, response);
}
setPickupState(IDLE);
}
void convert_to_bus(SL_Bus_etasetgett_std_msgs_MultiArrayDimension* busPtr, std_msgs::MultiArrayDimension const* msgPtr)
{
{
int numItemsToCopy = msgPtr->label.size();
if (numItemsToCopy > 128)
{
ROS_WARN_NAMED("etasetgett", "Truncating array '%s' in received message '%s' from %d to %d items", "label", "std_msgs/MultiArrayDimension", numItemsToCopy, 128);
numItemsToCopy = 128;
}
busPtr->Label_SL_Info.CurrentLength = static_cast<uint32_T>( numItemsToCopy );
for (int i=0; i < numItemsToCopy; i++)
{
busPtr->Label[i] = (uint8_T) msgPtr->label[i];
}
}
busPtr->Size = msgPtr->size;
busPtr->Stride = msgPtr->stride;
}
void convert_to_bus(SL_Bus_Hector2P2_std_msgs_Header* busPtr, std_msgs::Header const* msgPtr)
{
{
int numItemsToCopy = msgPtr->frame_id.size();
if (numItemsToCopy > 128)
{
ROS_WARN_NAMED("Hector2P2", "Truncating array '%s' in received message '%s' from %d to %d items", "frame_id", "std_msgs/Header", numItemsToCopy, 128);
numItemsToCopy = 128;
}
busPtr->FrameId_SL_Info.CurrentLength = static_cast<uint32_T>( numItemsToCopy );
for (int i=0; i < numItemsToCopy; i++)
{
busPtr->FrameId[i] = (uint8_T) msgPtr->frame_id[i];
}
}
busPtr->Seq = msgPtr->seq;
convert_to_bus(&busPtr->Stamp, &msgPtr->stamp);
}
void initialize(UAS &uas_)
{
uas = &uas_;
XmlRpc::XmlRpcValue map_dict;
if (!dist_nh.getParam("", map_dict)) {
ROS_WARN_NAMED("distance_sensor", "DS: plugin not configured!");
return;
}
ROS_ASSERT(map_dict.getType() == XmlRpc::XmlRpcValue::TypeStruct);
for (auto &pair : map_dict) {
ROS_DEBUG_NAMED("distance_sensor", "DS: initializing mapping for %s", pair.first.c_str());
auto it = DistanceSensorItem::create_item(this, pair.first);
if (it)
sensor_map[it->sensor_id] = it;
else
ROS_ERROR_NAMED("distance_sensor", "DS: bad config for %s", pair.first.c_str());
}
}
ompl_interface::ConstraintApproximationConstructionResults
ompl_interface::ConstraintsLibrary::addConstraintApproximation(
const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard,
const std::string& group, const planning_scene::PlanningSceneConstPtr& scene,
const ConstraintApproximationConstructionOptions& options)
{
ConstraintApproximationConstructionResults res;
ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, options.state_space_parameterization);
if (pc)
{
pc->clear();
pc->setPlanningScene(scene);
pc->setCompleteInitialState(scene->getCurrentState());
ros::WallTime start = ros::WallTime::now();
ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, options, res);
ROS_INFO_NAMED("constraints_library", "Spent %lf seconds constructing the database",
(ros::WallTime::now() - start).toSec());
if (ss)
{
ConstraintApproximationPtr ca(new ConstraintApproximation(
group, options.state_space_parameterization, options.explicit_motions, constr_hard,
group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) +
".ompldb",
ss, res.milestones));
if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end())
ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", ca->getName().c_str());
constraint_approximations_[ca->getName()] = ca;
res.approx = ca;
}
else
ROS_ERROR_NAMED("constraints_library", "Unable to construct constraint approximation for group '%s'",
group.c_str());
}
return res;
}
bool GazeboNoisyDepth::FillDepthImageHelper(const uint32_t rows_arg,
const uint32_t cols_arg,
const uint32_t step_arg,
const float *data_arg,
sensor_msgs::Image *image_msg) {
if(data_arg == nullptr){
ROS_WARN_NAMED("NoisyDepth", "Invalid data array received - nullptr.");
return false;
}
image_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
image_msg->height = rows_arg;
image_msg->width = cols_arg;
image_msg->step = sizeof(float) * cols_arg;
image_msg->data.resize(rows_arg * cols_arg * sizeof(float));
image_msg->is_bigendian = 0;
float *dest = (float *)(&(image_msg->data[0]));
memcpy(dest, data_arg, sizeof(float) * width * height);
noise_model->ApplyNoise(width, height, dest);
return true;
}
void OpenNIListener::processBagfile(std::string filename,
const std::string& visua_tpc,
const std::string& depth_tpc,
const std::string& points_tpc,
const std::string& cinfo_tpc,
const std::string& odom_tpc,
const std::string& tf_tpc)
{
ROS_INFO_NAMED("OpenNIListener", "Loading Bagfile %s", filename.c_str());
Q_EMIT iamBusy(4, "Loading Bagfile", 0);
{ //bag will be destructed after this block (hopefully frees memory for the optimizer)
rosbag::Bag input_bag;
try{
input_bag.open(filename, rosbag::bagmode::Read);
} catch(rosbag::BagIOException ex) {
ROS_FATAL("Opening Bagfile %s failed: %s Quitting!", filename.c_str(), ex.what());
ros::shutdown();
return;
}
ROS_INFO_NAMED("OpenNIListener", "Opened Bagfile %s", filename.c_str());
std::vector<std::string> topics;
topics = createTopicsVector(visua_tpc, depth_tpc, points_tpc, cinfo_tpc, odom_tpc, tf_tpc);
rosbag::View view(input_bag, rosbag::TopicQuery(topics));
Q_EMIT iamBusy(4, "Processing Bagfile", view.size());
// Simulate sending of the messages in the bagfile
std::deque<sensor_msgs::Image::ConstPtr> vis_images;
std::deque<sensor_msgs::Image::ConstPtr> dep_images;
std::deque<sensor_msgs::CameraInfo::ConstPtr> cam_infos;
std::deque<sensor_msgs::PointCloud2::ConstPtr> pointclouds;
std::deque<nav_msgs::OdometryConstPtr> odometries;
//ros::Time last_tf=ros::Time(0);
ros::Time last_tf=ros::TIME_MIN;
bool tf_available = false;
int counter = 0;
BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
Q_EMIT progress(4, "Processing Bagfile", counter++);
do{
usleep(10);
if(!ros::ok()) return;
} while(pause_);
ROS_INFO_NAMED("OpenNIListener", "Processing %s of type %s with timestamp %f", m.getTopic().c_str(), m.getDataType().c_str(), m.getTime().toSec());
if (m.getTopic() == odom_tpc || ("/" + m.getTopic() == odom_tpc)) {
ROS_INFO_NAMED("OpenNIListener", "Processing %s of type %s with timestamp %f", m.getTopic().c_str(), m.getDataType().c_str(), m.getTime().toSec());
nav_msgs::OdometryConstPtr odommsg = m.instantiate<nav_msgs::Odometry>();
if (odommsg) odometries.push_back(odommsg);
}
else if (m.getTopic() == visua_tpc || ("/" + m.getTopic() == visua_tpc))
{
sensor_msgs::Image::ConstPtr rgb_img = m.instantiate<sensor_msgs::Image>();
if (rgb_img) vis_images.push_back(rgb_img);
ROS_DEBUG("Found Message of %s", visua_tpc.c_str());
}
else if (m.getTopic() == depth_tpc || ("/" + m.getTopic() == depth_tpc))
{
sensor_msgs::Image::ConstPtr depth_img = m.instantiate<sensor_msgs::Image>();
//if (depth_img) depth_img_sub_->newMessage(depth_img);
if (depth_img) dep_images.push_back(depth_img);
ROS_DEBUG("Found Message of %s", depth_tpc.c_str());
}
else if (m.getTopic() == points_tpc || ("/" + m.getTopic() == points_tpc))
{
sensor_msgs::PointCloud2::ConstPtr pointcloud = m.instantiate<sensor_msgs::PointCloud2>();
//if (cam_info) cam_info_sub_->newMessage(cam_info);
if (pointcloud) pointclouds.push_back(pointcloud);
ROS_DEBUG("Found Message of %s", points_tpc.c_str());
}
else if (m.getTopic() == cinfo_tpc || ("/" + m.getTopic() == cinfo_tpc))
{
sensor_msgs::CameraInfo::ConstPtr cam_info = m.instantiate<sensor_msgs::CameraInfo>();
//if (cam_info) cam_info_sub_->newMessage(cam_info);
if (cam_info) cam_infos.push_back(cam_info);
ROS_DEBUG("Found Message of %s", cinfo_tpc.c_str());
}
else if (m.getTopic() == tf_tpc|| ("/" + m.getTopic() == tf_tpc)){
tf::tfMessage::ConstPtr tf_msg = m.instantiate<tf::tfMessage>();
if (tf_msg) {
tf_available = true;
addTFMessageDirectlyToTransformer(tf_msg, tflistener_);
last_tf = tf_msg->transforms[0].header.stamp;
last_tf -= ros::Duration(0.1);
}
}
if (last_tf == ros::TIME_MIN){//If not a valid time yet, set to something before first message's stamp
last_tf = m.getTime();
last_tf -= ros::Duration(0.1);
}
//last_tf = m.getTime();//FIXME: No TF -> no processing
while(!odometries.empty() && odometries.front()->header.stamp < last_tf){
ROS_INFO_NAMED("OpenNIListener", "Sending Odometry message");
odomCallback(odometries.front());
odometries.pop_front();
}
while(!vis_images.empty() && vis_images.front()->header.stamp < last_tf){
ROS_INFO_NAMED("OpenNIListener", "Forwarding buffered visual message from time %12f", vis_images.front()->header.stamp.toSec());
rgb_img_sub_->newMessage(vis_images.front());
vis_images.pop_front();
}
while(!dep_images.empty() && dep_images.front()->header.stamp < last_tf){
ROS_INFO_NAMED("OpenNIListener", "Forwarding buffered depth message from time %12f", dep_images.front()->header.stamp.toSec());
depth_img_sub_->newMessage(dep_images.front());
dep_images.pop_front();
}
while(!cam_infos.empty() && cam_infos.front()->header.stamp < last_tf){
ROS_INFO_NAMED("OpenNIListener", "Forwarding buffered cam info message from time %12f", cam_infos.front()->header.stamp.toSec());
cam_info_sub_->newMessage(cam_infos.front());
cam_infos.pop_front();
}
while(!pointclouds.empty() && pointclouds.front()->header.stamp < last_tf){
pc_sub_->newMessage(pointclouds.front());
pointclouds.pop_front();
}
}
ROS_WARN_NAMED("eval", "Finished processing of Bagfile");
input_bag.close();
}
void sendCommad(){
ros::Duration sleep_time(0.1);
bool error = false;
unsigned int num_req;
while(nh_.ok() && ros::ok() && !error){
num_req = getRequestNum();
if (num_req > 0){
KukaRequest req = getRequest();
pop(); // TODO
// Check option
switch (req.opt_){
case STOP:
// Send Stop
error = kuka_.stop();
ROS_WARN_NAMED(name_, "Sending stop request to KUKA robot.");
break;
case PTP:
// Send PTP command
kuka_.setJointPosition(req.data_.ptp_goal);
ROS_DEBUG_NAMED(name_, "Sending PTP request to KUKA robot.");
break;
case VEL:
// Send PTP command
kuka_.setOverrideSpeed(req.data_.vel);
ROS_DEBUG_NAMED(name_, "Sending velocity override request to KUKA robot.");
break;
// case HOME:
// // Send Home
// for(unsigned int i=0; i<6; i++) req.data_.ptp_goal[i]=0.0*rad2deg+offset_[i];
// kuka_.setJointPosition(req.data_.ptp_goal);
// ROS_DEBUG_NAMED(name_, "Sending HOME request to KUKA robot.");
// break;
default:
ROS_ERROR_NAMED(name_, "Undefined option type.");
}
sleep_time = ros::Duration(0.1);
}
// Update joint state
else if (num_req == 0) {
ROS_DEBUG_NAMED(name_, "Update request to KUKA robot.");
// Call KUKA update
kuka_.update();
// Reset delta position and norm values
double norm = 0.0;
for (unsigned int i = 0; i < 6; ++i) position_d_[i] = joint_state_.position[i];
// Get joint position
kuka_.getJointPosition(joint_state_.position);
// Offset and rad convertion
for (unsigned int i = 0; i < 6; ++i)
{
joint_state_.position[i] = (joint_state_.position[i] - offset_[i])*deg2rad;
position_d_[i] -= joint_state_.position[i];
norm += position_d_[i]*position_d_[i];
}
//norm = sqrt(norm);
if ( norm < 0.01 ) sleep_time = ros::Duration(1);
ROS_DEBUG_NAMED(name_, "Norma: %.2f", norm);
// Publish joint state
joint_state_.header.stamp = ros::Time::now();
joint_state_.header.seq++;
joint_state_pub_.publish(joint_state_);
}
ROS_DEBUG_THROTTLE_NAMED(0.2, name_, "Current number of req %i", getRequestNum());
sleep_time.sleep();
}
}
ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstraintApproximation(
const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling,
const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options,
ConstraintApproximationConstructionResults& result)
{
// state storage structure
ConstraintApproximationStateStorage* cass = new ConstraintApproximationStateStorage(pcontext->getOMPLStateSpace());
ob::StateStoragePtr sstor(cass);
// construct a sampler for the sampling constraints
kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel());
robot_state::Transforms no_transforms(pcontext->getRobotModel()->getModelFrame());
kset.add(constr_hard, no_transforms);
const robot_state::RobotState& default_state = pcontext->getCompleteInitialRobotState();
unsigned int attempts = 0;
double bounds_val = std::numeric_limits<double>::max() / 2.0 - 1.0;
pcontext->getOMPLStateSpace()->setPlanningVolume(-bounds_val, bounds_val, -bounds_val, bounds_val, -bounds_val,
bounds_val);
pcontext->getOMPLStateSpace()->setup();
// construct the constrained states
robot_state::RobotState robot_state(default_state);
const constraint_samplers::ConstraintSamplerManagerPtr& csmng = pcontext->getConstraintSamplerManager();
ConstrainedSampler* csmp = nullptr;
if (csmng)
{
constraint_samplers::ConstraintSamplerPtr cs =
csmng->selectSampler(pcontext->getPlanningScene(), pcontext->getJointModelGroup()->getName(), constr_sampling);
if (cs)
csmp = new ConstrainedSampler(pcontext.get(), cs);
}
ob::StateSamplerPtr ss(csmp ? ob::StateSamplerPtr(csmp) : pcontext->getOMPLStateSpace()->allocDefaultStateSampler());
ompl::base::ScopedState<> temp(pcontext->getOMPLStateSpace());
int done = -1;
bool slow_warn = false;
ompl::time::point start = ompl::time::now();
while (sstor->size() < options.samples)
{
++attempts;
int done_now = 100 * sstor->size() / options.samples;
if (done != done_now)
{
done = done_now;
ROS_INFO_NAMED("constraints_library", "%d%% complete (kept %0.1lf%% sampled states)", done,
100.0 * (double)sstor->size() / (double)attempts);
}
if (!slow_warn && attempts > 10 && attempts > sstor->size() * 100)
{
slow_warn = true;
ROS_WARN_NAMED("constraints_library", "Computation of valid state database is very slow...");
}
if (attempts > options.samples && sstor->size() == 0)
{
ROS_ERROR_NAMED("constraints_library", "Unable to generate any samples");
break;
}
ss->sampleUniform(temp.get());
pcontext->getOMPLStateSpace()->copyToRobotState(robot_state, temp.get());
if (kset.decide(robot_state).satisfied)
{
if (sstor->size() < options.samples)
{
temp->as<ModelBasedStateSpace::StateType>()->tag = sstor->size();
sstor->addState(temp.get());
}
}
}
result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start);
ROS_INFO_NAMED("constraints_library", "Generated %u states in %lf seconds", (unsigned int)sstor->size(),
result.state_sampling_time);
if (csmp)
{
result.sampling_success_rate = csmp->getConstrainedSamplingRate();
ROS_INFO_NAMED("constraints_library", "Constrained sampling rate: %lf", result.sampling_success_rate);
}
result.milestones = sstor->size();
if (options.edges_per_sample > 0)
{
ROS_INFO_NAMED("constraints_library", "Computing graph connections (max %u edges per sample) ...",
options.edges_per_sample);
// construct connexions
const ob::StateSpacePtr& space = pcontext->getOMPLSimpleSetup()->getStateSpace();
unsigned int milestones = sstor->size();
std::vector<ob::State*> int_states(options.max_explicit_points, nullptr);
pcontext->getOMPLSimpleSetup()->getSpaceInformation()->allocStates(int_states);
ompl::time::point start = ompl::time::now();
int good = 0;
int done = -1;
for (std::size_t j = 0; j < milestones; ++j)
{
int done_now = 100 * j / milestones;
if (done != done_now)
{
done = done_now;
ROS_INFO_NAMED("constraints_library", "%d%% complete", done);
}
if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
continue;
const ob::State* sj = sstor->getState(j);
for (std::size_t i = j + 1; i < milestones; ++i)
{
if (cass->getMetadata(i).first.size() >= options.edges_per_sample)
continue;
double d = space->distance(sstor->getState(i), sj);
if (d >= options.max_edge_length)
continue;
unsigned int isteps =
std::min<unsigned int>(options.max_explicit_points, d / options.explicit_points_resolution);
double step = 1.0 / (double)isteps;
bool ok = true;
space->interpolate(sstor->getState(i), sj, step, int_states[0]);
for (unsigned int k = 1; k < isteps; ++k)
{
double this_step = step / (1.0 - (k - 1) * step);
space->interpolate(int_states[k - 1], sj, this_step, int_states[k]);
pcontext->getOMPLStateSpace()->copyToRobotState(robot_state, int_states[k]);
if (!kset.decide(robot_state).satisfied)
{
ok = false;
break;
}
}
if (ok)
{
cass->getMetadata(i).first.push_back(j);
cass->getMetadata(j).first.push_back(i);
if (options.explicit_motions)
{
cass->getMetadata(i).second[j].first = sstor->size();
for (unsigned int k = 0; k < isteps; ++k)
{
int_states[k]->as<ModelBasedStateSpace::StateType>()->tag = -1;
sstor->addState(int_states[k]);
}
cass->getMetadata(i).second[j].second = sstor->size();
cass->getMetadata(j).second[i] = cass->getMetadata(i).second[j];
}
good++;
if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
break;
}
}
}
result.state_connection_time = ompl::time::seconds(ompl::time::now() - start);
ROS_INFO_NAMED("constraints_library", "Computed possible connexions in %lf seconds. Added %d connexions",
result.state_connection_time, good);
pcontext->getOMPLSimpleSetup()->getSpaceInformation()->freeStates(int_states);
return sstor;
}
// TODO(davetcoleman): this function did not originally return a value, causing compiler warnings in ROS Melodic
// Update with more intelligent logic as needed
ROS_ERROR_NAMED("constraints_library", "No StateStoragePtr found - implement better solution here.");
return sstor;
}
