virtual void onInit() {
std::string port; ROS_ASSERT_MSG(getPrivateNodeHandle().getParam("port", port),
"\"port\" param missing");
int baudrate = 115200; getPrivateNodeHandle().getParam("baudrate", baudrate);
ROS_ASSERT_MSG(getPrivateNodeHandle().getParam("frame_id", frame_id),
"\"frame_id\" param missing");
pub = getNodeHandle().advertise<uf_common::Float64Stamped>("depth", 10);
device = boost::make_shared<Device>(port, baudrate);
heartbeat_timer = getNodeHandle().createTimer(ros::Duration(0.5), boost::bind(&Nodelet::heartbeat_callback, this, _1));
running = true;
polling_thread_inst = boost::thread(boost::bind(&Nodelet::polling_thread, this));
}
void AsyncSpinnerImpl::stop()
{
boost::mutex::scoped_lock lock(mutex_);
if (!continue_)
return;
ROS_ASSERT_MSG(member_spinlock.owns_lock(),
"Async spinner's member lock doesn't own the global spinlock, hrm.");
ROS_ASSERT_MSG(member_spinlock.mutex() == &spinmutex,
"Async spinner's member lock owns a lock on the wrong mutex?!?!?");
member_spinlock.unlock();
continue_ = false;
threads_.join_all();
}
// Map Callback
void TOea_Planner::mapCB(const nav_msgs::OccupancyGrid::ConstPtr& map_msg)
{
map_received_ = true;
planner_state.data = hardware::BUSY;
state_pub_.publish(planner_state);
Astar_.world_map_.origin_yaw = tf::getYaw (map_msg->info.origin.orientation);
ROS_ASSERT_MSG(Astar_.world_map_.origin_yaw == 0, "Can't handle rotated maps"); //if map is rotated, quit programm
Astar_.world_map_.resolution = map_msg->info.resolution;
Astar_.world_map_.height = map_msg->info.height;
Astar_.world_map_.width = map_msg->info.width;
Astar_.world_map_.origin_x = map_msg->info.origin.position.x;
Astar_.world_map_.origin_y = map_msg->info.origin.position.y;
// number_cells = world_map.width*world_map.height;
Astar_.allocate();
Astar_.get_robot_padded_footprint();
Astar_.get_robot_high_cost_footprint(); // higher cost for cells close to obstacles
sensor_msgs::PointCloud2 pcd;
Astar_.SetGridFromMap(map_msg, pcd);
pcd_pub_.publish(pcd);
Astar_.AStarClear();
planner_state.data = hardware::IDLE;
state_pub_.publish(planner_state);
ROS_INFO_NAMED(logger_name_, "Planner is ready to receive goals");
}
void DynamicMovementPrimitiveGUI::labelSelected(QListWidgetItem* current, QListWidgetItem* previous)
{
simulate_button_->setEnabled(false);
execute_button_->setEnabled(false);
// if(widget_list_map_.at(current->listWidget()).getName().compare(TRAJECTORY_LIST_NAME) == 0)
if(current->listWidget() == trajectory_list_)
{
dmp_space_frame_->setEnabled(true);
task_radio_button_->setEnabled(true);
joint_radio_button_->setEnabled(true);
learn_button_->setEnabled(true);
}
// else if(widget_list_map_.at(current->listWidget()).getName().compare(DMP_LIST_NAME) == 0)
else if(current->listWidget() == dmp_list_)
{
simulate_button_->setEnabled(true);
execute_button_->setEnabled(true);
}
// else if(widget_list_map_.at(current->listWidget()).getName().compare(ROBOT_PART_LIST_NAME) == 0)
else if(current->listWidget() == robot_part_list_)
{
}
else
{
ROS_ASSERT_MSG(false, "Unknown list name >%s<. This should never happen.",
current->listWidget()->objectName().toStdString().c_str());
}
}
/**
* Loads a policy file.
*
* @return True on success.
*/
bool AlphaPolicy::loadFromFile(const char* path)
{
int action;
double alpha;
std::vector<double> alphas;
std::string line;
bool alpha_line = false; // True for alpha line, false for action line. (Simple state machine.)
// Open file.
std::ifstream policy_file;
policy_file.open(path);
if(!policy_file.good())
{
ROS_ERROR_STREAM("Unable to open file " << path);
return false;
}
// Check Policy has what it needs to read the file.
ROS_ASSERT_MSG(state_space_, "Requires StateSpace to be initialized!");
// Read file.
while(!policy_file.eof())
{
std::getline(policy_file, line);
if(policy_file.fail())
continue;
line = stripComment(line);
if(isBlank(line))
continue; // Skip line
std::stringstream line_ss(line);
// Action line
if(!alpha_line)
{
// Read element from line.
std::string action_str;
line_ss >> action_str;
if(line_ss.fail())
{
ROS_ERROR_STREAM("Invalid action line: '" << line << "'");
return false;
}
// Convert to action.
// Kludgy. @todo Change actions from ints to FeatureValues.
shared_ptr<FeatureValue> action_obj = action_space_->readFeatureValue(action_str);
action = boost::dynamic_pointer_cast<SimpleFeatureValue>(action_obj)->asInt();
if(!action_space_->isValid(SimpleFeatureValue(action)))
{
ROS_ERROR_STREAM("Invalid action: '" << action_str << "'");
return false;
}
actions_.push_back(action);
alpha_line = true;
}
// Alpha line
else if(alpha_line)
void Costmap2D::updateWorld(double robot_x, double robot_y,
const vector<Observation>& observations, const vector<Observation>& clearing_observations){
//reset the markers for inflation
memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
//make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
//raytrace freespace
raytraceFreespace(clearing_observations);
//if we raytrace X meters out... we must re-inflate obstacles within the containing square of that circle
double inflation_window_size = 2 * (max_raytrace_range_ + inflation_radius_);
//clear all non-lethal obstacles in preparation for re-inflation
clearNonLethal(robot_x, robot_y, inflation_window_size, inflation_window_size);
//reset the inflation window
resetInflationWindow(robot_x, robot_y, inflation_window_size + 2 * inflation_radius_, inflation_window_size + 2 * inflation_radius_, inflation_queue_, false);
//now we also want to add the new obstacles we've received to the cost map
updateObstacles(observations, inflation_queue_);
inflateObstacles(inflation_queue_);
}
bool Publisher::isLatched() const {
PublicationPtr publication_ptr;
if (impl_ && impl_->isValid()) {
publication_ptr =
TopicManager::instance()->lookupPublication(impl_->topic_);
} else {
ROS_ASSERT_MSG(false, "Call to isLatched() on an invalid Publisher");
throw ros::Exception("Call to isLatched() on an invalid Publisher");
}
if (publication_ptr) {
return publication_ptr->isLatched();
} else {
ROS_ASSERT_MSG(false, "Call to isLatched() on an invalid Publisher");
throw ros::Exception("Call to isLatched() on an invalid Publisher");
}
}
void Publisher::publish(const boost::function<SerializedMessage(void)>& serfunc, SerializedMessage& m) const
{
if (!impl_)
{
ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
return;
}
if (!impl_->isValid())
{
ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
return;
}
TopicManager::instance()->publish(impl_->topic_, serfunc, m);
}
int URDF_RBDL_Model::jointIndex(const std::string& name)
{
JointMap::const_iterator it = m_jointMap.find(name);
ROS_ASSERT_MSG(it != m_jointMap.end(), "Unknown joint '%s'", name.c_str());
return it->second;
}
int main(int argc, char **argv)
{
ROS_INFO_STREAM("Play a left arm trajectory stored in a file");
ROS_INFO_STREAM("Generaly the file's been created executing $>rostopic echo /robot/limb/left/joint_states -n 1 >> filename");
ROS_ASSERT_MSG(argc>1, "File name required as a parameter");
std::string filename = argv[1];
ros::init(argc, argv, "trajectory_palyer");
Client client("/sdk/robot/limb/left/follow_joint_trajectory", true); // true -> don't need to call ros::spin()
ROS_INFO("Waiting for connecting with the server...");
ROS_ASSERT_MSG(client.waitForServer(ros::Duration(10.0)),"Timeout. Server not available.");
ROS_INFO("CONNECTED");
std::vector<std::string> joint_manes(N_JOINTS);
joint_manes[0]="left_e0";
joint_manes[1]="left_e1";
joint_manes[2]="left_s0";
joint_manes[3]="left_s1";
joint_manes[4]="left_w0";
joint_manes[5]="left_w1";
joint_manes[6]="left_w2";
/*std::vector<trajectory_msgs::JointTrajectoryPoint> points = get_points_from_file(filename);
foreach (trajectory_msgs::JointTrajectoryPoint p, points) {
ROS_DEBUG_STREAM("Point: " << p);
}*/
// creating the trajectory
trajectory_msgs::JointTrajectory traj;
traj.joint_names=joint_manes;
traj.points = get_points_from_file(filename);
// Action goal
control_msgs::FollowJointTrajectoryGoal goal;
goal.trajectory = traj;
ROS_INFO_STREAM("Press ENTER to start trajectory. Whatch people around Baxter!!!");
std::cin.get();
goal.trajectory.header.stamp = ros::Time::now(); // start now
client.sendGoal(goal);
ROS_ASSERT_MSG(client.waitForResult(),"Home not reached.");
ROS_INFO("Home reached. Ready for next trajectory");
ROS_INFO_STREAM("action client state is " << client.getState().toString());
ros::shutdown();
}
std::string URDF_RBDL_Model::jointName(int index)
{
ROS_ASSERT_MSG(index >= 0 && index < (int)m_jointNames.size(),
"Invalid joint index %d", index
);
return m_jointNames[index];
}
void OctomapServerMultilayer::handlePreNodeTraversal(const ros::Time& rostime){
// multilayer server always publishes 2D maps:
m_publish2DMap = true;
nav_msgs::MapMetaData gridmapInfo = m_gridmap.info;
OctomapServer::handlePreNodeTraversal(rostime);
// recalculate height of arm layer (stub, TODO)
geometry_msgs::PointStamped vin;
vin.point.x = 0;
vin.point.y = 0;
vin.point.z = 0;
vin.header.stamp = rostime;
double link_padding = 0.03;
double minArmHeight = 2.0;
double maxArmHeight = 0.0;
for (unsigned i = 0; i < m_armLinks.size(); ++i){
vin.header.frame_id = m_armLinks[i];
geometry_msgs::PointStamped vout;
const bool found_trans =
m_tfListener.waitForTransform("base_footprint", m_armLinks.at(i),
ros::Time(0), ros::Duration(1.0));
ROS_ASSERT_MSG(found_trans, "Timed out waiting for transform to %s",
m_armLinks[i].c_str());
m_tfListener.transformPoint("base_footprint",vin,vout);
maxArmHeight = std::max(maxArmHeight, vout.point.z + (m_armLinkOffsets.at(i) + link_padding));
minArmHeight = std::min(minArmHeight, vout.point.z - (m_armLinkOffsets.at(i) + link_padding));
}
ROS_INFO("Arm layer interval adjusted to (%f,%f)", minArmHeight, maxArmHeight);
m_multiGridmap.at(2).minZ = minArmHeight;
m_multiGridmap.at(2).maxZ = maxArmHeight;
m_multiGridmap.at(2).z = (maxArmHeight+minArmHeight)/2.0;
// TODO: also clear multilevel maps in BBX region (see OctomapServer.cpp)?
bool mapInfoChanged = mapChanged(gridmapInfo, m_gridmap.info);
for (MultilevelGrid::iterator it = m_multiGridmap.begin(); it != m_multiGridmap.end(); ++it){
it->map.header = m_gridmap.header;
it->map.info = m_gridmap.info;
it->map.info.origin.position.z = it->z;
if (m_projectCompleteMap){
ROS_INFO("Map resolution changed, rebuilding complete 2D maps");
it->map.data.clear();
// init to unknown:
it->map.data.resize(it->map.info.width * it->map.info.height, -1);
} else if (mapInfoChanged){
adjustMapData(it->map, gridmapInfo);
}
}
}
void SNS_IK::initialize() {
ROS_ASSERT_MSG(m_chain.getNrOfJoints() == m_lower_bounds.rows(),
"SNS_IK: Number of joint lower bounds does not equal number of joints");
ROS_ASSERT_MSG(m_chain.getNrOfJoints() == m_upper_bounds.rows(),
"SNS_IK: Number of joint upper bounds does not equal number of joints");
ROS_ASSERT_MSG(m_chain.getNrOfJoints() == m_velocity.rows(),
"SNS_IK: Number of max joint velocity bounds does not equal number of joints");
ROS_ASSERT_MSG(m_chain.getNrOfJoints() == m_acceleration.rows(),
"SNS_IK: Number of max joint acceleration bounds does not equal number of joints");
ROS_ASSERT_MSG(m_chain.getNrOfJoints() == m_jointNames.size(),
"SNS_IK: Number of joint names does not equal number of joints");
// Populate a vector cooresponding to the type for every joint
for (std::size_t i = 0; i < m_chain.segments.size(); i++) {
std::string type = m_chain.segments[i].getJoint().getTypeName();
if (type.find("Rot") != std::string::npos) {
if (m_upper_bounds(m_types.size()) >= std::numeric_limits<float>::max() &&
m_lower_bounds(m_types.size()) <= std::numeric_limits<float>::lowest()){
m_types.push_back(SNS_IK::JointType::Continuous);
} else {
m_types.push_back(SNS_IK::JointType::Revolute);
}
} else if (type.find("Trans") != std::string::npos) {
m_types.push_back(SNS_IK::JointType::Prismatic);
}
}
ROS_ASSERT_MSG(m_types.size()==(unsigned int)m_lower_bounds.data.size(),
"SNS_IK: Could not determine joint limits for all non-continuous joints");
m_jacobianSolver = std::shared_ptr<KDL::ChainJntToJacSolver>(new KDL::ChainJntToJacSolver(m_chain));
ROS_ASSERT_MSG(setVelocitySolveType(m_solvetype),
"SNS_IK: Failed to create a new SNS velocity and position solver."); //TODO make loop rate configurable
}
void DecAudioProcessor::audioCB(dec_audio::AudioSampleConstPtr audio_sample)
{
ROS_ASSERT_MSG(audio_sample->data.size() == NUM_AUDIO_SIGNALS,
"Number of hardcoded audio signals >%i< does not match number of received audio signals >%i<.",
(int)NUM_AUDIO_SIGNALS, (int)audio_sample->data.size());
boost::mutex::scoped_lock lock(audio_sample_mutex_);
audio_receveived_counter_++;
audio_sample_ = *audio_sample;
}
/** @brief Starts the node.
* @param[in] argc Number of Arguments
* @param[in] argv Array of Arguments
* - [0] Program name
* - [1] Topic name for receiving LaserScans
* - [2] Topic name for publishing detected half circles
* - [3] Topic name for publishing OpenCV images
* - ... other ros-specific arguments
*/
int main(int argc, char **argv) {
ROS_ASSERT_MSG(argc > 5,
"Not enough arguments for topic names. 6 expected, %d given.",
argc);
ros::init(argc, argv, argv[1]);
ros::NodeHandle node(argv[1]);
float laserRange;
float minimumDistance;
int stretchFactor;
float halfCircleRadius;
float minCirclePercentage;
float maxInlierDist;
float maxCircleDensity;
int maxCirclePoints;
ROS_ASSERT(node.getParam("laserRange", laserRange));
ROS_ASSERT(node.getParam("minimumDistance", minimumDistance));
ROS_ASSERT(node.getParam("stretchFactor", stretchFactor));
ROS_ASSERT(node.getParam("halfCircleRadius", halfCircleRadius));
ROS_ASSERT(node.getParam("minCirclePercentage", minCirclePercentage));
ROS_ASSERT(node.getParam("maxInlierDist", maxInlierDist));
ROS_ASSERT(node.getParam("maxCircleDensity", maxCircleDensity));
ROS_ASSERT(node.getParam("maxCirclePoints", maxCirclePoints));
HalfCircleDetector detector(laserRange, minimumDistance, stretchFactor,
halfCircleRadius, minCirclePercentage,
maxInlierDist, maxCircleDensity, maxCirclePoints);
/** Subscribers */
ros::Subscriber sub = node.subscribe(
"/base_scan", 1, &HalfCircleDetector::receiveLaserScan, &detector);
/** Publishers */
ros::Publisher posePub = node.advertise< geometry_msgs::Pose2D >(argv[2], 1);
image_transport::ImageTransport it(node);
image_transport::Publisher imagePub = it.advertise(argv[3], 1);
ros::Rate rate(10);
while (ros::ok()) {
posePub.publish(detector.getHalfCirclePose());
sensor_msgs::ImagePtr imagePtr =
cv_bridge::CvImage(std_msgs::Header(), "mono8",
detector.getLaserScanImage()).toImageMsg();
imagePub.publish(imagePtr);
ros::spinOnce();
rate.sleep();
}
return 0;
}
Trajectory_Player::Trajectory_Player(const char * service_name)
{
_client = new Client(service_name, true);
ROS_INFO_STREAM("Waiting to connect to service " << service_name);
ROS_ASSERT_MSG(_client->waitForServer(ros::Duration(10.0)),"Timeout. Service not available. Is the trajectory controller running?");
ROS_INFO("Service CONNECTED");
_tip_collision.set(false);
if (std::string(service_name).find("left")!=std::string::npos) _gripper = new Vacuum_Gripper(left);
else _gripper = new Vacuum_Gripper(right);
}
MallocTLSInit()
{
int ret = pthread_key_create(&g_tls_key, tlsDestructor);
ROS_ASSERT_MSG(!ret, "Failed to create TLS");
for (size_t i = 0; i < MAX_ALLOC_INFO; ++i)
{
g_alloc_info_used[i].store(false);
}
g_tls_key_initialized = true;
}
SerialInterface::SerialInterface (std::string port, uint32_t speed):serialport_name_ (port), serialport_speed_ (speed)
{
struct termios tio;
status = false;
serialport_baud_ = bitrate (serialport_speed_);
ROS_INFO ("Initializing serial port...");
dev_ = open(serialport_name_.c_str (),O_RDWR | O_NOCTTY | O_NDELAY);
ROS_DEBUG ("dev: %d", dev_);
ROS_ASSERT_MSG (dev_ != -1, "Failed to open serial port: %s %s", serialport_name_.c_str (), strerror (errno));
ROS_ASSERT_MSG (tcgetattr (dev_, &tio) == 0, "Unknown Error: %s", strerror (errno));
cfsetispeed (&tio, serialport_baud_);
cfsetospeed (&tio, serialport_baud_);
tio.c_iflag = 0;
tio.c_iflag &= ~(BRKINT | ICRNL | IMAXBEL);
tio.c_iflag |= IGNBRK;
tio.c_oflag = 0;
tio.c_oflag &= ~(OPOST | ONLCR);
tio.c_cflag = (tio.c_cflag & ~CSIZE) | CS8;
tio.c_cflag &= ~(PARENB | CRTSCTS | CSTOPB);
tio.c_lflag = 0;
tio.c_lflag |= NOFLSH;
tio.c_lflag &= ~(ISIG | IEXTEN | ICANON | ECHO | ECHOE);
ROS_ASSERT_MSG (tcsetattr (dev_, TCSADRAIN, &tio) == 0, "Unknown Error: %s", strerror (errno));
tio.c_cc[VMIN] = 0;
tio.c_cc[VTIME] = 0;
tcflush (dev_, TCIOFLUSH);
ROS_ASSERT_MSG (dev_ != NULL, "Could not open serial port %s", serialport_name_.c_str ());
ROS_INFO ("Successfully connected to %s, Baudrate %d\n", serialport_name_.c_str (), serialport_speed_);
}
Vacuum_Gripper::Vacuum_Gripper(vacuum_gripper_type gripper)
{
_gripper=gripper;
_pub_command_grip = _nh.advertise<std_msgs::Float32>("/robot/limb/" + Vacuum_Gripper::type_to_str(_gripper) + "/accessory/gripper/command_grip", 1);
ROS_ASSERT_MSG(_pub_command_grip,"Empty publisher for gripping");
_pub_command_release = _nh.advertise<std_msgs::Empty>("/robot/limb/" + Vacuum_Gripper::type_to_str(_gripper) + "/accessory/gripper/command_release", 1);
ROS_ASSERT_MSG(_pub_command_release,"Empty publisher for releasing");
_sub_state = _nh.subscribe("/sdk/robot/limb/" + Vacuum_Gripper::type_to_str(_gripper) + "/accessory/gripper/state", 1, &Vacuum_Gripper::new_state_msg_handler, this);
baxter_msgs::GripperState initial_gripper_state; //Initially all the interesting properties of the state are unknown
initial_gripper_state.calibrated= \
initial_gripper_state.enabled= \
initial_gripper_state.error= \
initial_gripper_state.gripping= \
initial_gripper_state.missed= \
initial_gripper_state.ready= \
initial_gripper_state.moving=baxter_msgs::GripperState::STATE_UNKNOWN;
_state.set(initial_gripper_state);
}
int main(int argc, char ** argv)
{
ROS_INFO_STREAM("Concatenating several xml trajectory files into just one");
ROS_ASSERT_MSG(argc>3, "concat_trajectory_xml_files [dest_file] [orig_file1] [orig_file2] ... [orig_fileN]");
std::string dest_file = argv[1];
std::vector<trajectory_msgs::JointTrajectory> trajs;
std::vector<std::string> traj_ids;
for (int i = 2; i < argc; ++i) {
std::string orig_file=argv[i];
ROS_INFO_STREAM("Reading trajectories from file " << orig_file);
std::vector<trajectory_msgs::JointTrajectory> trajs_file;
std::vector<std::string> traj_ids_file;
if(!ttt::trajectory_xml_parser::read_from_file(orig_file,trajs_file,traj_ids_file))
{
ROS_ERROR("Error reading from file");
return -1;
}
ROS_ASSERT_MSG(trajs_file.size()==traj_ids_file.size(),"#trajs != #traj_ids");
ROS_INFO_STREAM(trajs_file.size() << " trajectories read");
trajs.insert(trajs.end(),trajs_file.begin(),trajs_file.end());
traj_ids.insert(traj_ids.end(),traj_ids_file.begin(),traj_ids_file.end());
trajs_file.clear();
traj_ids_file.clear();
}
ROS_INFO_STREAM("Writing " << trajs.size() << " trajectories to file " << dest_file);
if(!ttt::trajectory_xml_parser::write_to_file(trajs,dest_file,traj_ids))
{
ROS_ERROR("Error writing to file");
return -1;
}
return 0;
}
std::vector<trajectory_msgs::JointTrajectoryPoint> get_points_from_file(std::string filename)
{
ROS_DEBUG_STREAM("Reading positions from " << filename);
QFile file(filename.c_str());
ROS_ASSERT_MSG(file.open(QIODevice::ReadOnly | QIODevice::Text), "Error openning file to read positions");
std::vector<trajectory_msgs::JointTrajectoryPoint> points;
size_t counter = 1; // the first point starts 1 second after
while (!file.atEnd()) {
QByteArray line = file.readLine();
if (line.startsWith("position:")) //this line contains the positions of the joints
{
line.remove(0,10); //removing "position: "
line.replace('[',' '); //removing [
line.replace(']',' '); //removing ]
line=line.trimmed(); //removing white spaces
QList<QByteArray> joint_positions = line.split(',');
/*foreach (QByteArray token, joint_positions) {
ROS_DEBUG_STREAM("token = " << token.data());
}*/
//ROS_DEBUG_STREAM("positions read = " << line.data());
ROS_ASSERT_MSG(joint_positions.size()==N_JOINTS, "Incongruency in number of joints positions");
trajectory_msgs::JointTrajectoryPoint point;
point.positions.resize(N_JOINTS,0.0);
point.velocities.resize(N_JOINTS,0.0);
point.accelerations.resize(N_JOINTS,0.0);
bool ok=false;
for (int i = 0; i < N_JOINTS; i++) {
point.positions[i]=joint_positions[i].toDouble(&ok);
if (!ok) ROS_WARN_STREAM("Error converting to double " << joint_positions[i].data());
//ROS_DEBUG_STREAM("point.position[" << i << "]=" << point.positions[i] << " # joint_positions[" << i << "]=" << joint_positions[i].data() << "=>(toDouble)=>" << joint_positions[i].toDouble());
}
point.time_from_start=ros::Duration(TIME_GAP*counter++);
points.push_back(point);
}
}
return points;
}
void Costmap2D::reinflateWindow(double wx, double wy, double w_size_x, double w_size_y, bool clear){
//reset the markers for inflation
memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
//make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
//reset the inflation window.. adds all lethal costs to the queue for re-propagation
resetInflationWindow(wx, wy, w_size_x, w_size_y, inflation_queue_, clear);
//inflate the obstacles
inflateObstacles(inflation_queue_);
}
bool DynamicMovementPrimitiveGUI::getDMPName(std::string& dmp_name)
{
std::vector<task_recorder2_msgs::Description> dmp_descriptions;
// if(!widget_list_map_.at(dmp_list_).getDescriptions(dmp_descriptions))
getSelectedDescriptionsSignal(dmp_list_, dmp_descriptions);
if(dmp_descriptions.empty())
{
setStatusReport("No DMP selected...", ERROR);
return false;
}
ROS_ASSERT_MSG(dmp_descriptions.size() == 1, "There should only be 1 DMP selected. This should never happen.");
dmp_name = task_recorder2_utilities::getFileName(dmp_descriptions[0]);
return true;
}
void Costmap2D::replaceFullMap(double win_origin_x, double win_origin_y,
unsigned int data_size_x, unsigned int data_size_y,
const std::vector<unsigned char>& static_data){
//delete our old maps
deleteMaps();
//update the origin and size of the new map
size_x_ = data_size_x;
size_y_ = data_size_y;
origin_x_ = win_origin_x;
origin_y_ = win_origin_y;
//initialize our various maps
initMaps(size_x_, size_y_);
//make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
unsigned int index = 0;
unsigned char* costmap_index = costmap_;
std::vector<unsigned char>::const_iterator static_data_index = static_data.begin();
//copy static data into the costmap
for(unsigned int i = 0; i < size_y_; ++i){
for(unsigned int j = 0; j < size_x_; ++j){
//check if the static value is above the unknown or lethal thresholds
if(track_unknown_space_ && unknown_cost_value_ > 0 && *static_data_index == unknown_cost_value_)
*costmap_index = NO_INFORMATION;
else if(*static_data_index >= lethal_threshold_)
*costmap_index = LETHAL_OBSTACLE;
else
*costmap_index = FREE_SPACE;
if(*costmap_index == LETHAL_OBSTACLE){
unsigned int mx, my;
indexToCells(index, mx, my);
enqueue(index, mx, my, mx, my, inflation_queue_);
}
++costmap_index;
++static_data_index;
++index;
}
}
//now... let's inflate the obstacles
inflateObstacles(inflation_queue_);
//we also want to keep a copy of the current costmap as the static map
memcpy(static_map_, costmap_, size_x_ * size_y_ * sizeof(unsigned char));
}
int main(int argc, char** argv)
{
ROS_ASSERT_MSG(argc>=3,"[Move_Maker_Server] You need to specify the xml trajectory file and the service executing it.\nFor example: rosrun tictactoe %s ttt_moves.trajectories /sdk/robot/limb/left/follow_joint_trajectory",argv[0]);
std::string file=argv[1];
std::string service=argv[2];
ros::init(argc, argv, "ttt_move_maker");
ttt::Move_Maker baxter_moves(file.c_str(),service.c_str());
ros::Duration(2.0).sleep();
ros::spinOnce();
ROS_INFO("[Move_Maker_Server] Moving the left arm to home position.");
baxter_moves.make_a_move(std::vector<std::string> (1,"home"), std::vector<ttt::Trajectory_Type> (1,ttt::PLAIN));
ros::spin();
return 0;
}
// Recursively append menu and submenu entries to menu, based on a
// vector of menu entry id numbers describing the menu entries at the
// current level.
void InteractiveMarker::populateMenu( QMenu* menu, std::vector<uint32_t>& ids )
{
for( size_t id_index = 0; id_index < ids.size(); id_index++ )
{
uint32_t id = ids[ id_index ];
std::map< uint32_t, MenuNode >::iterator node_it = menu_entries_.find( id );
ROS_ASSERT_MSG(node_it != menu_entries_.end(), "interactive marker menu entry %u not found during populateMenu().", id);
MenuNode node = (*node_it).second;
if ( node.child_ids.empty() )
{
IntegerAction* action = new IntegerAction( makeMenuString( node.entry.title ),
menu,
(int) node.entry.id );
connect( action, SIGNAL( triggered( int )), this, SLOT( handleMenuSelect( int )));
menu->addAction( action );
}
else
{
void ARMultiPublisher::arInit()
{
arInitCparam(&cam_param_);
ROS_INFO("Camera parameters for ARMultiPublisher are:");
arParamDisp(&cam_param_);
if ((multi_marker_config_ = arMultiReadConfigFile(pattern_filename_)) == NULL)
{
ROS_ASSERT_MSG(false, "Could not load configurations for ARMultiPublisher.");
}
// load in the object data - trained markers and associated bitmap files
// if ((object = ar_object::read_ObjData(pattern_filename_, &objectnum)) == NULL)
// ROS_BREAK ();
num_total_markers_ = multi_marker_config_->marker_num;
ROS_INFO("Read >%i< objects from file.", num_total_markers_);
size_ = cvSize(cam_param_.xsize, cam_param_.ysize);
capture_ = cvCreateImage(size_, IPL_DEPTH_8U, 4);
}
void Costmap2D::resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y){
ROS_ASSERT_MSG(w_size_x >= 0 && w_size_y >= 0, "You cannot specify a negative size window");
double start_point_x = wx - w_size_x / 2;
double start_point_y = wy - w_size_y / 2;
double end_point_x = start_point_x + w_size_x;
double end_point_y = start_point_y + w_size_y;
//check start bounds
start_point_x = max(origin_x_, start_point_x);
start_point_y = max(origin_y_, start_point_y);
//check end bounds
end_point_x = min(origin_x_ + getSizeInMetersX(), end_point_x);
end_point_y = min(origin_y_ + getSizeInMetersY(), end_point_y);
unsigned int start_x, start_y, end_x, end_y;
//check for legality just in case
if(!worldToMap(start_point_x, start_point_y, start_x, start_y) || !worldToMap(end_point_x, end_point_y, end_x, end_y))
return;
ROS_ASSERT(end_x >= start_x && end_y >= start_y);
unsigned int cell_size_x = end_x - start_x;
unsigned int cell_size_y = end_y - start_y;
//we need a map to store the obstacles in the window temporarily
unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
//copy the local window in the costmap to the local map
copyMapRegion(costmap_, start_x, start_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
//now we'll reset the costmap to the static map
memcpy(costmap_, static_map_, size_x_ * size_y_ * sizeof(unsigned char));
//now we want to copy the local map back into the costmap
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
//clean up
delete[] local_map;
}
bool DecAudioProcessor::initialize(XmlRpc::XmlRpcValue& config)
{
float calibration_time_in_seconds = 0.0;
ROS_VERIFY(DecLightShowUtilities::getParam(config, "calibration_time_in_seconds", calibration_time_in_seconds));
ROS_ASSERT_MSG(calibration_time_in_seconds > data_->control_dt_, "Calibration time in seconds >%.2f< must be greater than >%.2f< seconds.",
calibration_time_in_seconds, data_->control_dt_);
calibration_counts_ = static_cast<unsigned int>(calibration_time_in_seconds / data_->control_dt_);
ROS_ASSERT(calibration_counts_ > 0);
calibration_counts_++;
audio_sample_.data.resize(NUM_AUDIO_SIGNALS, 0.0);
// do this last
const int SUBSCRIBER_BUFFER_SIZE = 1;
audio_sub_ = data_->node_handle_.subscribe("/AudioProcessor/audio_samples", SUBSCRIBER_BUFFER_SIZE, &DecAudioProcessor::audioCB, this);
ROS_VERIFY(DecLightShowUtilities::getParam(config, "volume_base", volume_base_));
ROS_VERIFY(DecLightShowUtilities::getParam(config, "volume_scale", volume_scale_));
return true;
}
bool DecSensorDecayProcessor::initialize(XmlRpc::XmlRpcValue& config)
{
float decay_delay_in_seconds = 0.0;
ROS_VERIFY(DecLightShowUtilities::getParam(config, "decay_delay_in_seconds", decay_delay_in_seconds));
ROS_ASSERT_MSG(decay_delay_in_seconds > data_->control_dt_, "Decay delay in seconds >%.2f< must be greater than >%.2f< seconds.",
decay_delay_in_seconds, data_->control_dt_);
window_size_ = static_cast<unsigned int>(decay_delay_in_seconds / data_->control_dt_);
ROS_ASSERT(window_size_ > 0);
window_size_++;
int decay_type = 0;
ROS_VERIFY(DecLightShowUtilities::getParam(config, "decay_type", decay_type));
ROS_ASSERT(decay_type >= LINEAR && decay_type < NUM_DECAY_TYPES);
decay_type_ = static_cast<DecayType>(decay_type);
indices_.resize(data_->total_num_sensors_, window_size_ - 1);
values_.resize(window_size_, 0.0);
switch (decay_type)
{
case LINEAR:
{
values_[0] = 1.0;
float decay = 1.0f/static_cast<float>(window_size_ - 1);
for (unsigned int i = 1; i < window_size_; ++i)
{
values_[i] = values_[i - 1] - decay;
}
break;
}
default:
{
ROS_ERROR("%s : Unknown decay type >%i<.", name_.c_str(), (int)decay_type_);
return false;
}
}
values_[window_size_ - 1] = 0.0;
return true;
}
