int main(int argc, char **argv)
{
ros::init (argc, argv, "workspace_analysis");
ros::AsyncSpinner spinner(1);
spinner.start();
// wait some time for everything to be loaded correctly...
ROS_INFO_STREAM("Waiting a few seconds to load the robot description correctly...");
sleep(3);
ROS_INFO_STREAM("Hope this was enough!");
/*Get some ROS params */
ros::NodeHandle node_handle("~");
double res_x, res_y, res_z;
double min_x, min_y, min_z;
double max_x, max_y, max_z;
double roll, pitch, yaw;
int max_attempts;
double joint_limits_penalty_multiplier;
std::string group_name;
if (!node_handle.getParam("min_x", min_x))
min_x = 0.0;
if (!node_handle.getParam("max_x", max_x))
max_x = 0.0;
if (!node_handle.getParam("res_x", res_x))
res_x = 0.1;
if (!node_handle.getParam("min_y", min_y))
min_y = 0.0;
if (!node_handle.getParam("max_y", max_y))
max_y = 0.0;
if (!node_handle.getParam("res_y", res_y))
res_y = 0.1;
if (!node_handle.getParam("min_z", min_z))
min_z = 0.0;
if (!node_handle.getParam("max_z", max_z))
max_z = 0.0;
if (!node_handle.getParam("res_z", res_z))
res_z = 0.1;
if (!node_handle.getParam("max_attempts", max_attempts))
max_attempts = 10000;
if (!node_handle.getParam("roll", roll))
roll = 0.0;
if (!node_handle.getParam("pitch", pitch))
pitch = 0.0;
if (!node_handle.getParam("yaw", yaw))
yaw = 0.0;
if (!node_handle.getParam("joint_limits_penalty_multiplier", joint_limits_penalty_multiplier))
joint_limits_penalty_multiplier = 0.0;
std::string filename;
if (!node_handle.getParam("filename", filename))
ROS_ERROR("Will not write to file");
std::string quat_filename;
if (!node_handle.getParam("quat_filename", quat_filename))
ROS_ERROR("Will not write to file");
if (!node_handle.getParam("group_name", group_name))
ROS_FATAL("Must have valid group name");
/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
/* Create a robot state*/
robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
if(!robot_model->hasJointModelGroup(group_name))
ROS_FATAL("Invalid group name: %s", group_name.c_str());
const robot_model::JointModelGroup* joint_model_group = robot_model->getJointModelGroup(group_name);
/* Construct a planning scene - NOTE: this is for illustration purposes only.
The recommended way to construct a planning scene is to use the planning_scene_monitor
to construct it for you.*/
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
moveit_msgs::WorkspaceParameters workspace;
workspace.min_corner.x = min_x;
workspace.min_corner.y = min_y;
workspace.min_corner.z = min_z;
workspace.max_corner.x = max_x;
workspace.max_corner.y = max_y;
workspace.max_corner.z = max_z;
/* Load the workspace analysis */
moveit_workspace_analysis::WorkspaceAnalysis workspace_analysis(planning_scene, true, joint_limits_penalty_multiplier);
ros::Time init_time;
moveit_workspace_analysis::WorkspaceMetrics metrics;
/* Compute the metrics */
bool use_vigir_rpy = true;
if (use_vigir_rpy) {
std::vector<geometry_msgs::Quaternion> orientations;
geometry_msgs::Quaternion quaternion;
//yaw = -M_PI*0.0;
//roll = M_PI*0.49;
// translate roll, pitch and yaw into a Quaternion
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);
geometry_msgs::Quaternion odom_quat;
tf::quaternionTFToMsg(q, quaternion);
orientations.push_back(quaternion);
metrics = workspace_analysis.computeMetrics(workspace, orientations, robot_state.get(), joint_model_group, res_x, res_y, res_z);
} else {
// load the set of quaternions
std::vector<geometry_msgs::Quaternion> orientations;
std::ifstream quat_file;
quat_file.open(quat_filename.c_str());
while(!quat_file.eof())
{
geometry_msgs::Quaternion temp_quat;
orientations.push_back(temp_quat);
}
init_time = ros::Time::now();
metrics = workspace_analysis.computeMetrics(workspace, orientations, robot_state.get(), joint_model_group, res_x, res_y, res_z);
if(metrics.points_.empty())
ROS_WARN_STREAM("No point to be written to file: consider changing the workspace, or recompiling moveit_workspace_analysis with a longer sleeping time at the beginning (if this could be the cause)");
}
//ros::WallDuration duration(100.0);
//moveit_workspace_analysis::WorkspaceMetrics metrics = workspace_analysis.computeMetricsFK(&(*robot_state), joint_model_group, max_attempts, duration);
if(!filename.empty())
if(!metrics.writeToFile(filename,",",false))
ROS_INFO("Could not write to file");
ros::Duration total_duration = ros::Time::now() - init_time;
ROS_INFO_STREAM("Total duration: " << total_duration.toSec() << "s");
ros::shutdown();
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "exploration");
ros::NodeHandle nh;
ros::Publisher trajectory_pub = nh.advertise < trajectory_msgs::MultiDOFJointTrajectory
> (mav_msgs::default_topics::COMMAND_TRAJECTORY, 5);
ROS_INFO("Started exploration");
std_srvs::Empty srv;
bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);
unsigned int i = 0;
// Trying to unpause Gazebo for 10 seconds.
while (i <= 10 && !unpaused) {
ROS_INFO("Wait for 1 second before trying to unpause Gazebo again.");
std::this_thread::sleep_for(std::chrono::seconds(1));
unpaused = ros::service::call("/gazebo/unpause_physics", srv);
++i;
}
if (!unpaused) {
ROS_FATAL("Could not wake up Gazebo.");
return -1;
} else {
ROS_INFO("Unpaused the Gazebo simulation.");
}
double dt = 1.0;
std::string ns = ros::this_node::getName();
if (!ros::param::get(ns + "/nbvp/dt", dt)) {
ROS_FATAL("Could not start exploration. Parameter missing! Looking for %s",
(ns + "/nbvp/dt").c_str());
return -1;
}
static int n_seq = 0;
trajectory_msgs::MultiDOFJointTrajectory samples_array;
mav_msgs::EigenTrajectoryPoint trajectory_point;
trajectory_msgs::MultiDOFJointTrajectoryPoint trajectory_point_msg;
// Wait for 5 seconds to let the Gazebo GUI show up.
ros::Duration(5.0).sleep();
// This is the initialization motion, necessary that the known free space allows the planning
// of initial paths.
ROS_INFO("Starting the planner: Performing initialization motion");
for (double i = 0; i <= 1.0; i = i + 0.1) {
nh.param<double>("wp_x", trajectory_point.position_W.x(), 0.0);
nh.param<double>("wp_y", trajectory_point.position_W.y(), 0.0);
nh.param<double>("wp_z", trajectory_point.position_W.z(), 1.0);
samples_array.header.seq = n_seq;
samples_array.header.stamp = ros::Time::now();
samples_array.points.clear();
n_seq++;
tf::Quaternion quat = tf::Quaternion(tf::Vector3(0.0, 0.0, 1.0), -M_PI * i);
trajectory_point.setFromYaw(tf::getYaw(quat));
mav_msgs::msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &trajectory_point_msg);
samples_array.points.push_back(trajectory_point_msg);
trajectory_pub.publish(samples_array);
ros::Duration(1.0).sleep();
}
trajectory_point.position_W.x() -= 0.5;
trajectory_point.position_W.y() -= 0.5;
samples_array.header.seq = n_seq;
samples_array.header.stamp = ros::Time::now();
samples_array.points.clear();
n_seq++;
mav_msgs::msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &trajectory_point_msg);
samples_array.points.push_back(trajectory_point_msg);
trajectory_pub.publish(samples_array);
ros::Duration(1.0).sleep();
// Start planning: The planner is called and the computed path sent to the controller.
int iteration = 0;
while (ros::ok()) {
ROS_INFO_THROTTLE(0.5, "Planning iteration %i", iteration);
nbvplanner::nbvp_srv planSrv;
planSrv.request.header.stamp = ros::Time::now();
planSrv.request.header.seq = iteration;
planSrv.request.header.frame_id = "world";
if (ros::service::call("nbvplanner", planSrv)) {
n_seq++;
if (planSrv.response.path.size() == 0) {
ros::Duration(1.0).sleep();
}
for (int i = 0; i < planSrv.response.path.size(); i++) {
samples_array.header.seq = n_seq;
samples_array.header.stamp = ros::Time::now();
samples_array.header.frame_id = "world";
samples_array.points.clear();
tf::Pose pose;
tf::poseMsgToTF(planSrv.response.path[i], pose);
double yaw = tf::getYaw(pose.getRotation());
trajectory_point.position_W.x() = planSrv.response.path[i].position.x;
trajectory_point.position_W.y() = planSrv.response.path[i].position.y;
// Add offset to account for constant tracking error of controller
trajectory_point.position_W.z() = planSrv.response.path[i].position.z + 0.25;
tf::Quaternion quat = tf::Quaternion(tf::Vector3(0.0, 0.0, 1.0), yaw);
trajectory_point.setFromYaw(tf::getYaw(quat));
mav_msgs::msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &trajectory_point_msg);
samples_array.points.push_back(trajectory_point_msg);
trajectory_pub.publish(samples_array);
ros::Duration(dt).sleep();
}
} else {
ROS_WARN_THROTTLE(1, "Planner not reachable");
ros::Duration(1.0).sleep();
}
iteration++;
}
}
void FeatureViewer::resetDetector()
{
gft_config_server_.reset();
star_config_server_.reset();
orb_config_server_.reset();
surf_config_server_.reset();
if (detector_type_ == "ORB")
{
ROS_INFO("Creating ORB detector");
feature_detector_.reset(new rgbdtools::OrbDetector());
orb_config_server_.reset(new
OrbDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/ORB")));
// dynamic reconfigure
OrbDetectorConfigServer::CallbackType f = boost::bind(
&FeatureViewer::orbReconfigCallback, this, _1, _2);
orb_config_server_->setCallback(f);
}
else if (detector_type_ == "SURF")
{
ROS_INFO("Creating SURF detector");
feature_detector_.reset(new rgbdtools::SurfDetector());
surf_config_server_.reset(new
SurfDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/SURF")));
// dynamic reconfigure
SurfDetectorConfigServer::CallbackType f = boost::bind(
&FeatureViewer::surfReconfigCallback, this, _1, _2);
surf_config_server_->setCallback(f);
}
else if (detector_type_ == "GFT")
{
ROS_INFO("Creating GFT detector");
feature_detector_.reset(new rgbdtools::GftDetector());
gft_config_server_.reset(new
GftDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/GFT")));
// dynamic reconfigure
GftDetectorConfigServer::CallbackType f = boost::bind(
&FeatureViewer::gftReconfigCallback, this, _1, _2);
gft_config_server_->setCallback(f);
}
else if (detector_type_ == "STAR")
{
ROS_INFO("Creating STAR detector");
feature_detector_.reset(new rgbdtools::StarDetector());
star_config_server_.reset(new
StarDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/STAR")));
// dynamic reconfigure
StarDetectorConfigServer::CallbackType f = boost::bind(
&FeatureViewer::starReconfigCallback, this, _1, _2);
star_config_server_->setCallback(f);
}
else
{
ROS_FATAL("%s is not a valid detector type! Using GFT", detector_type_.c_str());
feature_detector_.reset(new rgbdtools::GftDetector());
gft_config_server_.reset(new
GftDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/GFT")));
// dynamic reconfigure
GftDetectorConfigServer::CallbackType f = boost::bind(
&FeatureViewer::gftReconfigCallback, this, _1, _2);
gft_config_server_->setCallback(f);
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "roomba560_node");
ROS_INFO("Roomba for ROS %.2f", NODE_VERSION);
double last_x, last_y, last_yaw;
double vel_x, vel_y, vel_yaw;
double dt;
float last_charge = 0.0;
int time_remaining = -1;
ros::NodeHandle n;
ros::NodeHandle pn("~");
pn.param<std::string>("port", port, "/dev/ttyUSB0");
std::string base_frame_id;
std::string odom_frame_id;
pn.param<std::string>("base_frame_id", base_frame_id, "base_link");
pn.param<std::string>("odom_frame_id", odom_frame_id, "odom");
roomba = new irobot::OpenInterface(port.c_str());
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("/odom", 50);
ros::Publisher battery_pub = n.advertise<irobotcreate2::Battery>("/battery", 50);
ros::Publisher bumper_pub = n.advertise<irobotcreate2::Bumper>("/bumper", 50);
ros::Publisher buttons_pub = n.advertise<irobotcreate2::Buttons>("/buttons", 50);
ros::Publisher cliff_pub = n.advertise<irobotcreate2::RoombaIR>("/cliff", 50);
ros::Publisher irbumper_pub = n.advertise<irobotcreate2::RoombaIR>("/ir_bumper", 50);
ros::Publisher irchar_pub = n.advertise<irobotcreate2::IRCharacter>("/ir_character", 50);
ros::Publisher wheeldrop_pub = n.advertise<irobotcreate2::WheelDrop>("/wheel_drop", 50);
tf::TransformBroadcaster tf_broadcaster;
ros::Subscriber cmd_vel_sub = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, cmdVelReceived);
ros::Subscriber leds_sub = n.subscribe<irobotcreate2::Leds>("/leds", 1, ledsReceived);
ros::Subscriber digitleds_sub = n.subscribe<irobotcreate2::DigitLeds>("/digit_leds", 1, digitLedsReceived);
ros::Subscriber song_sub = n.subscribe<irobotcreate2::Song>("/song", 1, songReceived);
ros::Subscriber playsong_sub = n.subscribe<irobotcreate2::PlaySong>("/play_song", 1, playSongReceived);
ros::Subscriber mode_sub = n.subscribe<std_msgs::String>("/mode", 1, cmdModeReceived);
irobot::OI_Packet_ID sensor_packets[1] = {irobot::OI_PACKET_GROUP_100};
roomba->setSensorPackets(sensor_packets, 1, OI_PACKET_GROUP_100_SIZE);
if( roomba->openSerialPort(true) == 0) ROS_INFO("Connected to Roomba.");
else
{
ROS_FATAL("Could not connect to Roomba.");
ROS_BREAK();
}
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
bool first_loop=true;
ros::Rate r(10.0);
while(n.ok())
{
current_time = ros::Time::now();
last_x = roomba->odometry_x_;
last_y = roomba->odometry_y_;
last_yaw = roomba->odometry_yaw_;
if( roomba->getSensorPackets(100) == -1) ROS_ERROR("Could not retrieve sensor packets.");
else roomba->calculateOdometry();
dt = (current_time - last_time).toSec();
vel_x = (roomba->odometry_x_ - last_x)/dt;
vel_y = (roomba->odometry_y_ - last_y)/dt;
vel_yaw = (roomba->odometry_yaw_ - last_yaw)/dt;
// ******************************************************************************************
//first, we'll publish the transforms over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = odom_frame_id;
odom_trans.child_frame_id = base_frame_id;
odom_trans.transform.translation.x = roomba->odometry_x_;
odom_trans.transform.translation.y = roomba->odometry_y_;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);
tf_broadcaster.sendTransform(odom_trans);
//TODO: Finish brodcasting the tf for all the ir sensors on the Roomba
/*geometry_msgs::TransformStamped cliff_left_trans;
cliff_left_trans.header.stamp = current_time;
cliff_left_trans.header.frame_id = "base_link";
cliff_left_trans.child_frame_id = "base_cliff_left";
cliff_left_trans.transform.translation.x = 0.0;
cliff_left_trans.transform.translation.y = 0.0;
cliff_left_trans.transform.translation.z = 0.0;
cliff_left_trans.transform.rotation = ;
tf_broadcaster.sendTransform(cliff_left_trans); */
// ******************************************************************************************
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = odom_frame_id;
//set the position
odom.pose.pose.position.x = roomba->odometry_x_;
odom.pose.pose.position.y = roomba->odometry_y_;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);
//set the velocity
odom.child_frame_id = base_frame_id;
odom.twist.twist.linear.x = vel_x;
odom.twist.twist.linear.y = vel_y;
odom.twist.twist.angular.z = vel_yaw;
//publish the message
odom_pub.publish(odom);
// ******************************************************************************************
//publish battery
irobotcreate2::Battery battery;
battery.header.stamp = current_time;
battery.power_cord = roomba->power_cord_;
battery.dock = roomba->dock_;
battery.level = 100.0*(roomba->charge_/roomba->capacity_);
if(last_charge > roomba->charge_) time_remaining = (int)(battery.level/((last_charge-roomba->charge_)/roomba->capacity_)/dt)/60;
last_charge = roomba->charge_;
battery.time_remaining = time_remaining;
battery_pub.publish(battery);
// ******************************************************************************************
//publish bumpers
irobotcreate2::Bumper bumper;
bumper.left.header.stamp = current_time;
bumper.left.state = roomba->bumper_[LEFT];
bumper.right.header.stamp = current_time;
bumper.right.state = roomba->bumper_[RIGHT];
bumper_pub.publish(bumper);
// ******************************************************************************************
//publish buttons
irobotcreate2::Buttons buttons;
buttons.header.stamp = current_time;
buttons.clean = roomba->buttons_[BUTTON_CLEAN];
buttons.spot = roomba->buttons_[BUTTON_SPOT];
buttons.dock = roomba->buttons_[BUTTON_DOCK];
buttons.day = roomba->buttons_[BUTTON_DAY];
buttons.hour = roomba->buttons_[BUTTON_HOUR];
buttons.minute = roomba->buttons_[BUTTON_MINUTE];
buttons.schedule = roomba->buttons_[BUTTON_SCHEDULE];
buttons.clock = roomba->buttons_[BUTTON_CLOCK];
buttons_pub.publish(buttons);
// ******************************************************************************************
//publish cliff
irobotcreate2::RoombaIR cliff;
cliff.header.stamp = current_time;
cliff.header.frame_id = "base_cliff_left";
cliff.state = roomba->cliff_[LEFT];
cliff.signal = roomba->cliff_signal_[LEFT];
cliff_pub.publish(cliff);
cliff.header.frame_id = "base_cliff_front_left";
cliff.state = roomba->cliff_[FRONT_LEFT];
cliff.signal = roomba->cliff_signal_[FRONT_LEFT];
cliff_pub.publish(cliff);
cliff.header.frame_id = "base_cliff_front_right";
cliff.state = roomba->cliff_[FRONT_RIGHT];
cliff.signal = roomba->cliff_signal_[FRONT_RIGHT];
cliff_pub.publish(cliff);
cliff.header.frame_id = "base_cliff_right";
cliff.state = roomba->cliff_[RIGHT];
cliff.signal = roomba->cliff_signal_[RIGHT];
cliff_pub.publish(cliff);
// ******************************************************************************************
//publish irbumper
irobotcreate2::RoombaIR irbumper;
irbumper.header.stamp = current_time;
irbumper.header.frame_id = "base_irbumper_left";
irbumper.state = roomba->ir_bumper_[LEFT];
irbumper.signal = roomba->ir_bumper_signal_[LEFT];
irbumper_pub.publish(irbumper);
irbumper.header.frame_id = "base_irbumper_front_left";
irbumper.state = roomba->ir_bumper_[FRONT_LEFT];
irbumper.signal = roomba->ir_bumper_signal_[FRONT_LEFT];
irbumper_pub.publish(irbumper);
irbumper.header.frame_id = "base_irbumper_center_left";
irbumper.state = roomba->ir_bumper_[CENTER_LEFT];
irbumper.signal = roomba->ir_bumper_signal_[CENTER_LEFT];
irbumper_pub.publish(irbumper);
irbumper.header.frame_id = "base_irbumper_center_right";
irbumper.state = roomba->ir_bumper_[CENTER_RIGHT];
irbumper.signal = roomba->ir_bumper_signal_[CENTER_RIGHT];
irbumper_pub.publish(irbumper);
irbumper.header.frame_id = "base_irbumper_front_right";
irbumper.state = roomba->ir_bumper_[FRONT_RIGHT];
irbumper.signal = roomba->ir_bumper_signal_[FRONT_RIGHT];
irbumper_pub.publish(irbumper);
irbumper.header.frame_id = "base_irbumper_right";
irbumper.state = roomba->ir_bumper_[RIGHT];
irbumper.signal = roomba->ir_bumper_signal_[RIGHT];
irbumper_pub.publish(irbumper);
// ******************************************************************************************
//publish irchar
irobotcreate2::IRCharacter irchar;
irchar.header.stamp = current_time;
irchar.omni = roomba->ir_char_[OMNI];
irchar.left = roomba->ir_char_[LEFT];
irchar.right = roomba->ir_char_[RIGHT];
irchar_pub.publish(irchar);
// ******************************************************************************************
//publish wheeldrop
irobotcreate2::WheelDrop wheeldrop;
wheeldrop.left.header.stamp = current_time;
wheeldrop.left.state = roomba->wheel_drop_[LEFT];
wheeldrop.right.header.stamp = current_time;
wheeldrop.right.state = roomba->wheel_drop_[RIGHT];
wheeldrop_pub.publish(wheeldrop);
ros::spinOnce();
r.sleep();
if(first_loop) {roomba->startOI(true); first_loop=false;}
}
roomba->powerDown();
roomba->closeSerialPort();
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
world = _model->GetWorld();
// load parameters
if (!_sdf->HasElement("robotNamespace"))
namespace_.clear();
else
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
if (!_sdf->HasElement("topicName"))
velocity_topic_ = "cmd_vel";
else
velocity_topic_ = _sdf->GetElement("topicName")->Get<std::string>();
if (!_sdf->HasElement("takeoffTopic"))
takeoff_topic_ = "/ardrone/takeoff";
else
takeoff_topic_ = _sdf->GetElement("takeoffTopic")->Get<std::string>();
if (!_sdf->HasElement("/ardrone/land"))
land_topic_ = "/ardrone/land";
else
land_topic_ = _sdf->GetElement("landTopic")->Get<std::string>();
if (!_sdf->HasElement("resetTopic"))
reset_topic_ = "/ardrone/reset";
else
reset_topic_ = _sdf->GetElement("resetTopic")->Get<std::string>();
if (!_sdf->HasElement("navdataTopic"))
navdata_topic_ = "/ardrone/navdata";
else
navdata_topic_ = _sdf->GetElement("navdataTopic")->Get<std::string>();
if (!_sdf->HasElement("imuTopic"))
imu_topic_.clear();
else
imu_topic_ = _sdf->GetElement("imuTopic")->Get<std::string>();
if (!_sdf->HasElement("sonarTopic"))
sonar_topic_.clear();
else
sonar_topic_ = _sdf->GetElement("sonarTopic")->Get<std::string>();
if (!_sdf->HasElement("contactTopic"))
contact_topic_.clear();
else
contact_topic_ = _sdf->GetElement("contactTopic")->Get<std::string>();
if (!_sdf->HasElement("stateTopic"))
state_topic_.clear();
else
state_topic_ = _sdf->GetElement("stateTopic")->Get<std::string>();
if (!_sdf->HasElement("bodyName"))
{
link = _model->GetLink();
link_name_ = link->GetName();
}
else {
link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
link = boost::dynamic_pointer_cast<physics::Link>(world->GetEntity(link_name_));
}
if (!link)
{
ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
return;
}
node_handle_ = new ros::NodeHandle(namespace_);
// subscribe command: velocity control command
if (!velocity_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<geometry_msgs::Twist>(
velocity_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::VelocityCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
velocity_subscriber_ = node_handle_->subscribe(ops);
}
// subscribe command: take off command
if (!takeoff_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
takeoff_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::TakeoffCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
takeoff_subscriber_ = node_handle_->subscribe(ops);
}
// subscribe command: take off command
if (!land_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
land_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::LandCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
land_subscriber_ = node_handle_->subscribe(ops);
}
// subscribe command: take off command
if (!reset_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
reset_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::ResetCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
reset_subscriber_ = node_handle_->subscribe(ops);
}
m_navdataPub = node_handle_->advertise< ardrone_autonomy::Navdata >( navdata_topic_ , 25 );
// subscribe imu
if (!imu_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(
imu_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::ImuCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
imu_subscriber_ = node_handle_->subscribe(ops);
ROS_INFO_NAMED("quadrotor_state_controller", "Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str());
}
// subscribe sonar senor info
if (!sonar_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Range>(
sonar_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::SonarCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
sonar_subscriber_ = node_handle_->subscribe(ops);
ROS_INFO_NAMED("quadrotor_state_controller", "Using sonar information on topic %s as source of altitude.", sonar_topic_.c_str());
}
// subscribe contact senor info
if (!contact_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Int32>(
contact_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::ContactCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
contact_subscriber_ = node_handle_->subscribe(ops);
ROS_INFO_NAMED("quadrotor_state_controller", "Using contact information on topic %s as source of collisions.", contact_topic_.c_str());
}
// subscribe state
if (!state_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(
state_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::StateCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
state_subscriber_ = node_handle_->subscribe(ops);
ROS_INFO_NAMED("quadrotor_state_controller", "Using state information on topic %s as source of state information.", state_topic_.c_str());
}
// for camera control
// switch camera server
std::string toggleCam_topic = "ardrone/togglecam";
ros::AdvertiseServiceOptions toggleCam_ops = ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
toggleCam_topic,
boost::bind(&GazeboQuadrotorStateController::toggleCamCallback, this, _1,_2),
ros::VoidPtr(),
&callback_queue_);
toggleCam_service = node_handle_->advertiseService(toggleCam_ops);
// camera image data
std::string cam_out_topic = "/ardrone/image_raw";
std::string cam_front_in_topic = "/ardrone/front/image_raw";
std::string cam_bottom_in_topic = "/ardrone/bottom/image_raw";
std::string in_transport = "raw";
camera_it_ = new image_transport::ImageTransport(*node_handle_);
camera_publisher_ = camera_it_->advertise(cam_out_topic, 1);
camera_front_subscriber_ = camera_it_->subscribe(
cam_front_in_topic, 1,
boost::bind(&GazeboQuadrotorStateController::CameraFrontCallback, this, _1),
ros::VoidPtr(), in_transport);
camera_bottom_subscriber_ = camera_it_->subscribe(
cam_bottom_in_topic, 1,
boost::bind(&GazeboQuadrotorStateController::CameraBottomCallback, this, _1),
ros::VoidPtr(), in_transport);
// camera image data
std::string cam_info_out_topic = "/ardrone/camera_info";
std::string cam_info_front_in_topic = "/ardrone/front/camera_info";
std::string cam_info_bottom_in_topic = "/ardrone/bottom/camera_info";
camera_info_publisher_ = node_handle_->advertise<sensor_msgs::CameraInfo>(cam_info_out_topic,1);
ros::SubscribeOptions cam_info_front_ops = ros::SubscribeOptions::create<sensor_msgs::CameraInfo>(
cam_info_front_in_topic, 1,
boost::bind(&GazeboQuadrotorStateController::CameraInfoFrontCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
camera_info_front_subscriber_ = node_handle_->subscribe(cam_info_front_ops);
ros::SubscribeOptions cam_info_bottom_ops = ros::SubscribeOptions::create<sensor_msgs::CameraInfo>(
cam_info_bottom_in_topic, 1,
boost::bind(&GazeboQuadrotorStateController::CameraInfoBottomCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
camera_info_bottom_subscriber_ = node_handle_->subscribe(cam_info_bottom_ops);
// callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorStateController::CallbackQueueThread,this ) );
Reset();
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboQuadrotorStateController::Update, this));
robot_current_state = LANDED_MODEL;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "roomba560_light_node");
ROS_INFO("Roomba for ROS %.2f", NODE_VERSION);
double last_x, last_y, last_yaw;
double vel_x, vel_y, vel_yaw;
double dt;
ros::NodeHandle n;
ros::NodeHandle private_nh("~");
private_nh.param<std::string>("port", port, "/dev/ttyUSB0");
private_nh.param<double>("test_vel", test_vel, 0.1);
private_nh.param<double>("test_odom_th", test_odom_th, 0.02);
test_vel_inc = test_vel / 20;
roomba = new irobot::OpenInterface(port.c_str());
ros::Publisher js_pub = n.advertise<sensor_msgs::JointState>("/joint_states", 50);
sensor_msgs::JointState js;
js.name.resize(2);
js.position.resize(2);
js.velocity.resize(2);
js.effort.resize(2);
js.name[0] = "left_wheel_joint";
js.name[1] = "right_wheel_joint";
js.position[0] = 0.0;
js.position[1] = 0.0;
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("/odom", 5);
//tf::TransformBroadcaster odom_broadcaster;
ros::Subscriber cmd_vel_sub = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, cmdVelReceived);
if( roomba->openSerialPort(true) == 0) ROS_INFO("Connected to Roomba.");
else
{
ROS_FATAL("Could not connect to Roomba.");
ROS_BREAK();
}
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
int heartvalue = 0;
bool inc = true;
bool packets_received = false;
unsigned int packets_not_received = 0;
unsigned int test_cnt = 0;
double test_vel_tmp = 0.0;
ros::Rate r(20.0);
while(n.ok())
{
current_time = ros::Time::now();
last_x = roomba->odometry_x_;
last_y = roomba->odometry_y_;
last_yaw = roomba->odometry_yaw_;
if (!command_tested) {
if (test_vel_tmp<test_vel) test_vel_tmp += test_vel_inc;
roomba->drive(test_vel_tmp,0.0);
roomba->setLeds(0, 0, 0, 0, 255, 255);
if (roomba->odometry_x_ > test_odom_th) {
roomba->drive(0.0,0.0);
command_tested = true;
ROS_INFO("Test of base driver was successful.");
} else {
if (test_cnt++ >= 20) { // 2s
roomba->powerDown();
roomba->closeSerialPort();
roomba_is_down = true;
ROS_ERROR("Test of base driver failed. Shutting down. Please press red button.");
ros::Duration dur(10);
dur.sleep();
break;
}
}
} else {
if(inc==true) heartvalue += 20;
else heartvalue -= 20;
if(heartvalue>=255) {
heartvalue = 255;
inc=false;
}
if(heartvalue<=0) {
heartvalue = 0;
inc=true;
}
roomba->setLeds(0, 0, 0, 0, (unsigned char)heartvalue, 255);
}
if( (roomba->getSensorPackets(100) == -1) && (packets_not_received++ >= 5) ) {
roomba->powerDown();
roomba->closeSerialPort();
roomba_is_down = true;
ROS_ERROR("Could not retrieve sensor packets. Shutting down. Please reset robot.");
ros::Duration dur(20);
dur.sleep();
break;
} else {
if (!packets_received) {
ROS_INFO("We are receiving sensor packets!");
packets_received = true;
}
packets_not_received = 0;
roomba->calculateOdometry();
}
dt = (current_time - last_time).toSec();
if (command_tested) roomba->drive(g_lin, g_ang);
vel_x = (roomba->odometry_x_ - last_x)/dt;
vel_y = (roomba->odometry_y_ - last_y)/dt;
vel_yaw = (roomba->odometry_yaw_ - last_yaw)/dt;
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(roomba->odometry_yaw_);
//first, we'll publish the transform over tf
/*geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = roomba->odometry_x_;
odom_trans.transform.translation.y = roomba->odometry_y_;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;*/
//send the transform
//odom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
//odom.header.frame_id = "odom";
odom.header.frame_id = "odom_combined";
//set the position
odom.pose.pose.position.x = roomba->odometry_x_;
odom.pose.pose.position.y = roomba->odometry_y_;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
//set the velocity
//odom.child_frame_id = "base_link";
odom.child_frame_id = "base_footprint";
odom.twist.twist.linear.x = vel_x;
odom.twist.twist.linear.y = vel_y;
odom.twist.twist.angular.z = vel_yaw;
// ZdenekM -> added covariance, needed for robot_pose_ekf...
odom.pose.covariance = boost::assign::list_of (1e-3) (0) (0) (0) (0) (0)
(0) (1e-3) (0) (0) (0) (0)
(0) (0) (1e6) (0) (0) (0)
(0) (0) (0) (1e6) (0) (0)
(0) (0) (0) (0) (1e6) (0)
(0) (0) (0) (0) (0) (1e3) ;
odom.twist.covariance = boost::assign::list_of (1e-3) (0) (0) (0) (0) (0)
(0) (1e-3) (0) (0) (0) (0)
(0) (0) (1e6) (0) (0) (0)
(0) (0) (0) (1e6) (0) (0)
(0) (0) (0) (0) (1e6) (0)
(0) (0) (0) (0) (0) (1e3) ;
//publish the message
if (command_tested) {
odom_pub.publish(odom);
// stop if there is no command for more than 0.5s
if ((ros::Time::now() - latest_command) > ros::Duration(0.5)) {
g_ang = 0.0;
g_lin = 0.0;
roomba->drive(0.0,0.0);
}
}
js_pub.publish(js);
ros::spinOnce();
r.sleep();
}
roomba->setLeds(0, 0, 0, 0, 0, 0);
if (!roomba_is_down) {
roomba->powerDown();
roomba->closeSerialPort();
}
}
int main(int argc, char ** argv)
{
//Initialize ROS
ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName);
if(argc == 11)
{
ros::Duration sleeper(atof(argv[10])/1000.0);
if (strcmp(argv[8], argv[9]) == 0)
ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[8], argv[9]);
TransformSender tf_sender(atof(argv[1]), atof(argv[2]), atof(argv[3]),
atof(argv[4]), atof(argv[5]), atof(argv[6]), atof(argv[7]),
ros::Time() + sleeper, //Future dating to allow slower sending w/o timeout
argv[8], argv[9]);
while(tf_sender.node_.ok())
{
tf_sender.send(ros::Time::now() + sleeper);
ROS_DEBUG("Sending transform from %s with parent %s\n", argv[8], argv[9]);
sleeper.sleep();
}
return 0;
}
else if (argc == 10)
{
ros::Duration sleeper(atof(argv[9])/1000.0);
if (strcmp(argv[7], argv[8]) == 0)
ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[7], argv[8]);
TransformSender tf_sender(atof(argv[1]), atof(argv[2]), atof(argv[3]),
atof(argv[4]), atof(argv[5]), atof(argv[6]),
ros::Time() + sleeper, //Future dating to allow slower sending w/o timeout
argv[7], argv[8]);
while(tf_sender.node_.ok())
{
tf_sender.send(ros::Time::now() + sleeper);
ROS_DEBUG("Sending transform from %s with parent %s\n", argv[7], argv[8]);
sleeper.sleep();
}
return 0;
}
else
{
printf("A command line utility for manually sending a transform.\n");
printf("It will periodicaly republish the given transform. \n");
printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period(milliseconds) \n");
printf("OR \n");
printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period(milliseconds) \n");
printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n");
printf("of the child_frame_id. \n");
ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments");
return -1;
}
};
void ARSinglePublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg)
{
ARUint8 *dataPtr;
ARMarkerInfo *marker_info;
int marker_num;
int i, k;
/* Get the image from ROSTOPIC
* NOTE: the dataPtr format is BGR because the ARToolKit library was
* build with V4L, dataPtr format change according to the
* ARToolKit configure option (see config.h).*/
try
{
capture_ = bridge_.imgMsgToCv (image_msg, "bgr8");
}
catch (sensor_msgs::CvBridgeException & e)
{
ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ());
}
//cvConvertImage(capture_,capture_,CV_CVTIMG_FLIP); //flip image
dataPtr = (ARUint8 *) capture_->imageData;
// detect the markers in the video frame
if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0)
{
ROS_FATAL ("arDetectMarker failed");
ROS_BREAK (); // FIXME: I don't think this should be fatal... -Bill
}
// check for known patterns
k = -1;
for (i = 0; i < marker_num; i++)
{
if (marker_info[i].id == patt_id_)
{
ROS_DEBUG ("Found pattern: %d ", patt_id_);
// make sure you have the best pattern (highest confidence factor)
if (k == -1)
k = i;
else if (marker_info[k].cf < marker_info[i].cf)
k = i;
}
}
if (k != -1)
{
// **** get the transformation between the marker and the real camera
double arQuat[4], arPos[3];
if (!useHistory_ || contF == 0)
arGetTransMat (&marker_info[k], marker_center_, markerWidth_, marker_trans_);
else
arGetTransMatCont (&marker_info[k], marker_trans_, marker_center_, markerWidth_, marker_trans_);
contF = 1;
//arUtilMatInv (marker_trans_, cam_trans);
arUtilMat2QuatPos (marker_trans_, arQuat, arPos);
// **** convert to ROS frame
double quat[4], pos[3];
pos[0] = arPos[0] * AR_TO_ROS;
pos[1] = arPos[1] * AR_TO_ROS;
pos[2] = arPos[2] * AR_TO_ROS;
quat[0] = -arQuat[0];
quat[1] = -arQuat[1];
quat[2] = -arQuat[2];
quat[3] = arQuat[3];
ROS_DEBUG (" QUAT: Pos x: %3.5f y: %3.5f z: %3.5f", pos[0], pos[1], pos[2]);
ROS_DEBUG (" Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]);
// **** publish the marker
ar_pose_marker_.header.frame_id = image_msg->header.frame_id;
ar_pose_marker_.header.stamp = image_msg->header.stamp;
ar_pose_marker_.id = marker_info->id;
ar_pose_marker_.pose.pose.position.x = pos[0];
ar_pose_marker_.pose.pose.position.y = pos[1];
ar_pose_marker_.pose.pose.position.z = pos[2];
ar_pose_marker_.pose.pose.orientation.x = quat[0];
ar_pose_marker_.pose.pose.orientation.y = quat[1];
ar_pose_marker_.pose.pose.orientation.z = quat[2];
ar_pose_marker_.pose.pose.orientation.w = quat[3];
ar_pose_marker_.confidence = marker_info->cf;
arMarkerPub_.publish(ar_pose_marker_);
ROS_DEBUG ("Published ar_single marker");
// **** publish transform between camera and marker
btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]);
btVector3 origin(pos[0], pos[1], pos[2]);
btTransform t(rotation, origin);
if(publishTf_)
{
if(reverse_transform_)
{
tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame_.c_str(), image_msg->header.frame_id);
broadcaster_.sendTransform(markerToCam);
} else {
tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame_.c_str());
broadcaster_.sendTransform(camToMarker);
}
}
// **** publish visual marker
if(publishVisualMarkers_)
{
btVector3 markerOrigin(0, 0, 0.25 * markerWidth_ * AR_TO_ROS);
btTransform m(btQuaternion::getIdentity(), markerOrigin);
btTransform markerPose = t * m; // marker pose in the camera frame
tf::poseTFToMsg(markerPose, rvizMarker_.pose);
rvizMarker_.header.frame_id = image_msg->header.frame_id;
rvizMarker_.header.stamp = image_msg->header.stamp;
rvizMarker_.id = 1;
rvizMarker_.scale.x = 1.0 * markerWidth_ * AR_TO_ROS;
rvizMarker_.scale.y = 1.0 * markerWidth_ * AR_TO_ROS;
rvizMarker_.scale.z = 0.5 * markerWidth_ * AR_TO_ROS;
rvizMarker_.ns = "basic_shapes";
rvizMarker_.type = visualization_msgs::Marker::CUBE;
rvizMarker_.action = visualization_msgs::Marker::ADD;
rvizMarker_.color.r = 0.0f;
rvizMarker_.color.g = 1.0f;
rvizMarker_.color.b = 0.0f;
rvizMarker_.color.a = 1.0;
rvizMarker_.lifetime = ros::Duration();
rvizMarkerPub_.publish(rvizMarker_);
ROS_DEBUG ("Published visual marker");
}
}
else
{
contF = 0;
ROS_DEBUG ("Failed to locate marker");
}
}
bool TreeKinematics::readJoints(urdf::Model &robot_model,
KDL::Tree &kdl_tree,
std::string &tree_root_name,
unsigned int &nr_of_jnts,
KDL::JntArray &joint_min,
KDL::JntArray &joint_max,
KDL::JntArray &joint_vel_max)
{
KDL::SegmentMap segmentMap;
segmentMap = kdl_tree.getSegments();
tree_root_name = kdl_tree.getRootSegment()->second.segment.getName();
nr_of_jnts = kdl_tree.getNrOfJoints();
ROS_DEBUG( "the tree's number of joints: [%d]", nr_of_jnts );
joint_min.resize(nr_of_jnts);
joint_max.resize(nr_of_jnts);
joint_vel_max.resize(nr_of_jnts);
info_.joint_names.resize(nr_of_jnts);
info_.limits.resize(nr_of_jnts);
// The following walks through all tree segments, extracts their joints except joints of KDL::None and extracts
// the information about min/max position and max velocity of the joints not of type urdf::Joint::UNKNOWN or
// urdf::Joint::FIXED
ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None.");
boost::shared_ptr<const urdf::Joint> joint;
for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it)
{
if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
{
// check, if joint can be found in the URDF model of the robot
joint = robot_model.getJoint(seg_it->second.segment.getJoint().getName().c_str());
if (!joint)
{
ROS_FATAL("Joint '%s' has not been found in the URDF robot model!", joint->name.c_str());
return false;
}
// extract joint information
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
{
ROS_DEBUG( "getting information about joint: [%s]", joint->name.c_str() );
double lower = 0.0, upper = 0.0, vel_limit = 0.0;
unsigned int has_pos_limits = 0, has_vel_limits = 0;
if ( joint->type != urdf::Joint::CONTINUOUS )
{
ROS_DEBUG("joint is not continuous.");
lower = joint->limits->lower;
upper = joint->limits->upper;
has_pos_limits = 1;
if (joint->limits->velocity)
{
has_vel_limits = 1;
vel_limit = joint->limits->velocity;
ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
}
else
{
has_vel_limits = 0;
vel_limit = 0.0;
ROS_DEBUG("joint has no velocity limit.");
}
}
else
{
ROS_DEBUG("joint is continuous.");
lower = -M_PI;
upper = M_PI;
has_pos_limits = 0;
if(joint->limits && joint->limits->velocity)
{
has_vel_limits = 1;
vel_limit = joint->limits->velocity;
ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
}
else
{
has_vel_limits = 0;
vel_limit = 0.0;
ROS_DEBUG("joint has no velocity limit.");
}
}
joint_min(seg_it->second.q_nr) = lower;
joint_max(seg_it->second.q_nr) = upper;
joint_vel_max(seg_it->second.q_nr) = vel_limit;
ROS_DEBUG("pos_min = %f, pos_max = %f, vel_max = %f", lower, upper, vel_limit);
info_.joint_names[seg_it->second.q_nr] = joint->name;
info_.limits[seg_it->second.q_nr].joint_name = joint->name;
info_.limits[seg_it->second.q_nr].has_position_limits = has_pos_limits;
info_.limits[seg_it->second.q_nr].min_position = lower;
info_.limits[seg_it->second.q_nr].max_position = upper;
info_.limits[seg_it->second.q_nr].has_velocity_limits = has_vel_limits;
info_.limits[seg_it->second.q_nr].max_velocity = vel_limit;
}
}
}
return true;
}
OPTCalibrationNode::OPTCalibrationNode(const ros::NodeHandle & node_handle)
: node_handle_(node_handle),
image_transport_(node_handle),
world_computation_(FIRST_SENSOR),
fixed_sensor_pose_(Pose::Identity())
{
action_sub_ = node_handle_.subscribe("action", 1, &OPTCalibrationNode::actionCallback, this);
status_pub_ = node_handle_.advertise<opt_msgs::CalibrationStatus>("status", 1);
std::string world_computation_s;
node_handle_.param("world_computation", world_computation_s, std::string("first_sensor"));
if (world_computation_s == "first_sensor")
world_computation_ = FIRST_SENSOR;
else if (world_computation_s == "last_checkerboard")
world_computation_ = LAST_CHECKERBOARD;
else if (world_computation_s == "update")
world_computation_ = UPDATE;
else
ROS_FATAL_STREAM("\"world_computation\" parameter value not valid. Please use \"first_sensor\", \"last_checkerboard\" or \"manual\".");
std::string fixed_sensor_frame_id;
if (world_computation_ == UPDATE)
{
if (not node_handle_.getParam("fixed_sensor/name", fixed_sensor_frame_id))
ROS_FATAL_STREAM("No \"fixed_sensor/name\" parameter found!!");
std::string pose_s = "/poses/" + fixed_sensor_frame_id;
if (not node_handle_.hasParam(pose_s))
ROS_FATAL_STREAM("No \"" << pose_s << "\" parameter found!! Has \"conf/camera_poses.yaml\" been loaded with \"rosparam load ...\"?");
Translation3 t;
node_handle_.getParam(pose_s + "/translation/x", t.x());
node_handle_.getParam(pose_s + "/translation/y", t.y());
node_handle_.getParam(pose_s + "/translation/z", t.z());
Quaternion q;
node_handle_.getParam(pose_s + "/rotation/x", q.x());
node_handle_.getParam(pose_s + "/rotation/y", q.y());
node_handle_.getParam(pose_s + "/rotation/z", q.z());
node_handle_.getParam(pose_s + "/rotation/w", q.w());
fixed_sensor_pose_.linear() = q.toRotationMatrix();
fixed_sensor_pose_.translation() = t.vector();
}
node_handle_.param("num_sensors", num_sensors_, 0);
double cell_width, cell_height;
int rows, cols;
bool cb_ok = true;
cb_ok = cb_ok and node_handle_.getParam("cell_width", cell_width);
cb_ok = cb_ok and node_handle_.getParam("cell_height", cell_height);
cb_ok = cb_ok and node_handle_.getParam("rows", rows);
cb_ok = cb_ok and node_handle_.getParam("cols", cols);
if (not cb_ok)
ROS_FATAL("Checkerboard parameter missing! Please set \"rows\", \"cols\", \"cell_width\" and \"cell_height\".");
checkerboard_ = boost::make_shared<Checkerboard>(cols, rows, cell_width, cell_height);
checkerboard_->setFrameId("/checkerboard");
for (int i = 0; i < num_sensors_; ++i)
{
std::stringstream ss;
std::string type_s;
ss.str("");
ss << "sensor_" << i << "/type";
if (not node_handle_.getParam(ss.str(), type_s))
ROS_FATAL_STREAM("No \"" << ss.str() << "\" parameter found!!");
// SensorNode::SensorType type;
// if (type_s == "pinhole_rgb")
// type = SensorNode::PINHOLE_RGB;
// else if (type_s == "kinect_depth")
// type = SensorNode::KINECT_DEPTH;
// else if (type_s == "tof_depth")
// type = SensorNode::TOF_DEPTH;
// else
// ROS_FATAL_STREAM("\"" << ss.str() << "\" parameter value not valid. Please use \"pinhole_rgb\", \"kinect_depth\" or \"tof_depth\".");
ss.str("");
ss << "/sensor_" << i;
std::string frame_id = ss.str();
ss.str("");
ss << "sensor_" << i << "/name";
node_handle_.param(ss.str(), frame_id, frame_id);
ROSDevice::Ptr ros_device;
if (type_s == "pinhole_rgb")
{
PinholeRGBDevice::Ptr device = boost::make_shared<PinholeRGBDevice>(frame_id);
pinhole_vec_.push_back(device);
ros_device = device;
ROS_INFO_STREAM(device->frameId() << " added.");
if (world_computation_ == UPDATE)
{
ROS_INFO_STREAM(frame_id << " == " << fixed_sensor_frame_id << "?");
if (frame_id == fixed_sensor_frame_id)
fixed_sensor_ = device->sensor();
}
}
else if (type_s == "kinect1")
{
KinectDevice::Ptr device = boost::make_shared<KinectDevice>(frame_id, frame_id + "_depth");
kinect_vec_.push_back(device);
ros_device = device;
if (world_computation_ == UPDATE)
{
ROS_INFO_STREAM(frame_id << " == " << fixed_sensor_frame_id << "?");
if (frame_id == fixed_sensor_frame_id)
fixed_sensor_ = device->colorSensor();
}
}
else if (type_s == "sr4500")
{
SwissRangerDevice::Ptr device = boost::make_shared<SwissRangerDevice>(frame_id);
swiss_ranger_vec_.push_back(device);
ros_device = device;
if (world_computation_ == UPDATE)
{
ROS_INFO_STREAM(frame_id << " == " << fixed_sensor_frame_id << "?");
if (frame_id == fixed_sensor_frame_id)
fixed_sensor_ = device->depthSensor();
}
}
else
{
ROS_FATAL_STREAM("\"" << ss.str() << "\" parameter value not valid. Please use \"pinhole_rgb\", \"kinect\" or \"swiss_ranger\".");
}
ss.str("");
ss << "sensor_" << i;
ros_device->createSubscribers(node_handle_, image_transport_, ss.str());
}
if (world_computation_ == UPDATE and not fixed_sensor_)
ROS_FATAL_STREAM("Wrong \"fixed_sensor/name\" parameter provided: " << fixed_sensor_frame_id);
}
int main(int argc, char **argv)
{
// Init the ROS node
ros::init (argc, argv, "move_right_arm_joint_goal_test");
// Precondition: Valid clock
ros::NodeHandle nh;
if (!ros::Time::waitForValid(ros::WallDuration(5.0))) // NOTE: Important when using simulated clock
{
ROS_FATAL("Timed-out waiting for valid time.");
return EXIT_FAILURE;
}
// Action client for sending motion planing requests
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm_torso", true);
// Wait for the action client to be connected to the server
ROS_INFO("Connecting to server...");
if (!move_arm.waitForServer(ros::Duration(5.0)))
{
ROS_ERROR("Timed-out waiting for the move_arm action server.");
return EXIT_FAILURE;
}
ROS_INFO("Connected to server.");
// Prepare motion plan request with joint-space goal
arm_navigation_msgs::MoveArmGoal goal;
std::vector<std::string> names(9);
names[0] = "torso_1_joint";
names[1] = "torso_2_joint";
names[2] = "arm_right_1_joint";
names[3] = "arm_right_2_joint";
names[4] = "arm_right_3_joint";
names[5] = "arm_right_4_joint";
names[6] = "arm_right_5_joint";
names[7] = "arm_right_6_joint";
names[8] = "arm_right_7_joint";
goal.motion_plan_request.group_name = "right_arm_torso";
goal.motion_plan_request.num_planning_attempts = 1;
goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
goal.motion_plan_request.planner_id= std::string("");
goal.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
goal.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
for (unsigned int i = 0 ; i < goal.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
{
goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.05;
goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.05;
}
goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0;
goal.motion_plan_request.goal_constraints.joint_constraints[1].position = 0.0;
goal.motion_plan_request.goal_constraints.joint_constraints[2].position = 1.2;
goal.motion_plan_request.goal_constraints.joint_constraints[3].position = 0.15;
goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -1.5708;
goal.motion_plan_request.goal_constraints.joint_constraints[5].position = 1.3;
goal.motion_plan_request.goal_constraints.joint_constraints[6].position = 0.0;
goal.motion_plan_request.goal_constraints.joint_constraints[7].position = 0.0;
goal.motion_plan_request.goal_constraints.joint_constraints[8].position = 0.0;
// Send motion plan request
if (nh.ok())
{
bool finished_within_time = false;
move_arm.sendGoal(goal);
finished_within_time = move_arm.waitForResult(ros::Duration(15.0));
if (!finished_within_time)
{
move_arm.cancelGoal();
ROS_INFO("Timed out achieving joint-space goal.");
}
else
{
actionlib::SimpleClientGoalState state = move_arm.getState();
bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
if(success)
ROS_INFO("Action finished: %s",state.toString().c_str());
else
ROS_INFO("Action failed: %s",state.toString().c_str());
}
}
ros::shutdown();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, std::string("owd"));
#ifdef OWD_RT
if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1){
ROS_FATAL("owd: mlockall failed: ");
return NULL;
}
#else // OWD_RT
#ifndef OWDSIM
// increase the process priority so that it can still do effective control
// on a non-realtime system
if (setpriority(PRIO_PROCESS,0,-5)) {
ROS_WARN("Could not elevate process scheduling priority; will be more vulnerable to heartbeat faults on a heavily loaded system");
ROS_WARN("Please make sure the executable is set to suid root");
}
#endif // OWDSIM
#endif // OWD_RT
// read parameters and set wam options
ros::NodeHandle n("~");
std::string calibration_filename;
int canbus_number;
std::string hand_type;
bool forcetorque;
bool modified_j1;
int pub_freq; // DEPRECATED
int wam_pub_freq;
int hand_pub_freq;
int ft_pub_freq;
int tactile_pub_freq;
n.param("calibration_file",calibration_filename,std::string("wam_joint_calibrations"));
n.param("canbus_number",canbus_number,0);
n.param("hand_type",hand_type,std::string("none"));
n.param("forcetorque_sensor",forcetorque,false);
n.param("modified_j1",modified_j1,false);
int wam_freq_default=10;
int hand_freq_default=10;
if (n.getParam("publish_frequency",pub_freq)) {
ROS_WARN("Parameter publish_frequency has been deprecated. Please use wam_publish_frequency, hand_publish_frequency, ft_publish_frequency, and tactile_publish_frequency.");
// we'll use the deprecated value to set the defaults for the
// individual hand and wam frequencies, so that it will only be
// used if the newer params are not specified
wam_freq_default=hand_freq_default=pub_freq;
}
n.param("wam_publish_frequency",wam_pub_freq,wam_freq_default);
n.param("hand_publish_frequency",hand_pub_freq,hand_freq_default);
n.param("ft_publish_frequency",ft_pub_freq,10);
n.param("tactile_publish_frequency",tactile_pub_freq,10);
if (wam_pub_freq > 500) {
ROS_WARN("value of wam_publish_frequency exceeds maximum sensor rate; capping to 500Hz");
wam_pub_freq = 500;
}
if (hand_pub_freq > 40) {
ROS_WARN("value of hand_publish_frequency exceeds maximum sensor rate; capping to 40Hz");
hand_pub_freq = 40;
}
if (ft_pub_freq > 500) {
ROS_WARN("value of ft_publish_frequency exceeds maximum sensor rate; capping to 500Hz");
ft_pub_freq = 500;
}
if (tactile_pub_freq > 40) {
ROS_WARN("value of tactile_publish_frequency exceeds maximum sensor rate; capping to 40Hz");
tactile_pub_freq = 40;
}
ROS_DEBUG("Using CANbus number %d",canbus_number);
int BH_model(0);
bool tactile(false);
if (! hand_type.compare(0,3,"280")) {
BH_model=280;
if (! hand_type.compare("280+TACT")) {
ROS_DEBUG("Expecting tactile sensors on this hand");
tactile=true;
}
} else if (! hand_type.compare(0,3,"260")) {
BH_model=260;
} else if (! hand_type.compare(0,7,"Robotiq")) {
BH_model=998;
} else if (! hand_type.compare(0,29,"darpa_arms_calibration_target")) {
BH_model=999;
} else if (hand_type.compare(0,4, "none")) { // note the absence of the !
ROS_FATAL("Unknown hand type \"%s\"; cannot continue with unknown mass properties",
hand_type.c_str());
exit(1);
}
}
// load robot footprint from costmap_2d_ros to keep same footprint format
std::vector<geometry_msgs::Point> CollisionVelocityFilter::loadRobotFootprint(ros::NodeHandle node){
std::vector<geometry_msgs::Point> footprint;
geometry_msgs::Point pt;
double padding;
std::string padding_param, footprint_param;
if(!node.searchParam("footprint_padding", padding_param))
padding = 0.01;
else
node.param(padding_param, padding, 0.01);
//grab the footprint from the parameter server if possible
XmlRpc::XmlRpcValue footprint_list;
std::string footprint_string;
std::vector<std::string> footstring_list;
if(node.searchParam("footprint", footprint_param)){
node.getParam(footprint_param, footprint_list);
if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) {
footprint_string = std::string(footprint_list);
//if there's just an empty footprint up there, return
if(footprint_string == "[]" || footprint_string == "")
return footprint;
boost::erase_all(footprint_string, " ");
boost::char_separator<char> sep("[]");
boost::tokenizer<boost::char_separator<char> > tokens(footprint_string, sep);
footstring_list = std::vector<std::string>(tokens.begin(), tokens.end());
}
//make sure we have a list of lists
if(!(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_list.size() > 2) && !(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString && footstring_list.size() > 5)){
ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", footprint_param.c_str(), std::string(footprint_list).c_str());
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
}
if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for(int i = 0; i < footprint_list.size(); ++i){
//make sure we have a list of lists of size 2
XmlRpc::XmlRpcValue point = footprint_list[i];
if(!(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2)){
ROS_FATAL("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form");
}
//make sure that the value we're looking at is either a double or an int
if(!(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
ROS_FATAL("Values in the footprint specification must be numbers");
throw std::runtime_error("Values in the footprint specification must be numbers");
}
pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]);
pt.x += sign(pt.x) * padding;
//make sure that the value we're looking at is either a double or an int
if(!(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
ROS_FATAL("Values in the footprint specification must be numbers");
throw std::runtime_error("Values in the footprint specification must be numbers");
}
pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]);
pt.y += sign(pt.y) * padding;
footprint.push_back(pt);
node.deleteParam(footprint_param);
std::ostringstream oss;
bool first = true;
BOOST_FOREACH(geometry_msgs::Point p, footprint) {
if(first) {
oss << "[[" << p.x << "," << p.y << "]";
first = false;
}
else {
oss << ",[" << p.x << "," << p.y << "]";
}
}
oss << "]";
node.setParam(footprint_param, oss.str().c_str());
node.setParam("footprint", oss.str().c_str());
}
}
else if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) {
int main(int argc, char** argv)
{
ros::init(argc, argv, "raw_marker_bridge");
ROS_INFO("Starting raw marker bridge...");
ros::NodeHandle nh;
ros::NodeHandle nhp("~");
ROS_INFO("Loading parameters...");
std::string tracker_hostname;
std::string tracker_port;
std::string tracker_frame_name;
std::string tracker_name;
std::string tracker_topic;
std::string stream_mode;
nhp.param(std::string("tracker_hostname"), tracker_hostname, std::string("192.168.2.161"));
nhp.param(std::string("tracker_port"), tracker_port, std::string("801"));
nhp.param(std::string("tracker_frame_name"), tracker_frame_name, std::string("mocap_world"));
nhp.param(std::string("tracker_name"), tracker_name, std::string("vicon"));
nhp.param(std::string("tracker_topic"), tracker_topic, std::string("mocap_markers"));
nhp.param(std::string("stream_mode"), stream_mode, std::string("ServerPush"));
// Check the stream mode
if (stream_mode == std::string("ServerPush") || stream_mode == std::string("ClientPullPreFetch") || stream_mode == std::string("ClientPull"))
{
ROS_INFO("Starting in %s mode", stream_mode.c_str());
}
else
{
ROS_FATAL("Invalid stream mode %s, valid options are ServerPush, ClientPullPreFetch, and ClientPull", stream_mode.c_str());
exit(-1);
}
// Assemble the full hostname
tracker_hostname = tracker_hostname + ":" + tracker_port;
ROS_INFO("Connecting to Vicon Tracker (DataStream SDK) at hostname %s", tracker_hostname.c_str());
// Make the ROS publisher
ros::Publisher mocap_pub = nh.advertise<lightweight_vicon_bridge::MocapMarkerArray>(tracker_topic, 1, false);
// Initialize the DataStream SDK
ViconDataStreamSDK::CPP::Client sdk_client;
ROS_INFO("Connecting to server...");
sdk_client.Connect(tracker_hostname);
usleep(10000);
while (!sdk_client.IsConnected().Connected && ros::ok())
{
ROS_WARN("...taking a while to connect, trying again...");
sdk_client.Connect(tracker_hostname);
usleep(10000);
}
ROS_INFO("...connected!");
// Enable data
sdk_client.EnableUnlabeledMarkerData();
// Set the axes (right-handed, X-forwards, Y-left, Z-up, same as ROS)
sdk_client.SetAxisMapping(ViconDataStreamSDK::CPP::Direction::Forward, ViconDataStreamSDK::CPP::Direction::Left, ViconDataStreamSDK::CPP::Direction::Up);
// Set streaming mode
if (stream_mode == "ServerPush")
{
sdk_client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush);
}
else if (stream_mode == "ClientPullPreFetch")
{
sdk_client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch);
}
else if (stream_mode == "ClientPull")
{
sdk_client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPull);
}
else
{
ROS_FATAL("Invalid stream mode");
exit(-2);
}
// Start streaming data
ROS_INFO("Streaming data...");
while (ros::ok())
{
// Get a new frame and process it
if (sdk_client.GetFrame().Result == ViconDataStreamSDK::CPP::Result::Success)
{
double total_latency = sdk_client.GetLatencyTotal().Total;
ros::Duration latency_duration(total_latency);
ros::Time current_time = ros::Time::now();
ros::Time frame_time = current_time - latency_duration;
lightweight_vicon_bridge::MocapMarkerArray state_msg;
state_msg.header.frame_id = tracker_frame_name;
state_msg.header.stamp = frame_time;
state_msg.tracker_name = tracker_name;
// Get the unlabeled markers
unsigned int unlabelled_markers = sdk_client.GetUnlabeledMarkerCount().MarkerCount;
for (unsigned int idx = 0; idx < unlabelled_markers; idx++)
{
ViconDataStreamSDK::CPP::Output_GetUnlabeledMarkerGlobalTranslation position = sdk_client.GetUnlabeledMarkerGlobalTranslation(idx);
lightweight_vicon_bridge::MocapMarker marker_msg;
marker_msg.index = idx;
marker_msg.position.x = position.Translation[0] * 0.001;
marker_msg.position.y = position.Translation[1] * 0.001;
marker_msg.position.z = position.Translation[2] * 0.001;
state_msg.markers.push_back(marker_msg);
}
mocap_pub.publish(state_msg);
}
// Handle ROS stuff
ros::spinOnce();
}
sdk_client.DisableUnlabeledMarkerData();
sdk_client.Disconnect();
return 0;
}
void CommonDataSubscriber::setupRGBDCallbacks(
ros::NodeHandle & nh,
ros::NodeHandle & pnh,
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
{
ROS_INFO("Setup rgbd callback");
if(subscribeOdom || subscribeUserData || subscribeScan2d || subscribeScan3d || subscribeOdomInfo)
{
rgbdSubs_.resize(1);
rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
rgbdSubs_[0]->subscribe(nh, "rgbd_image", 1);
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(nh, "odom", 1);
userDataSub_.subscribe(nh, "user_data", 1);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL4(rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
}
}
else if(subscribeOdom)
{
odomSub_.subscribe(nh, "odom", 1);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL3(rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
}
}
else if(subscribeUserData)
{
userDataSub_.subscribe(nh, "user_data", 1);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL3(rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
}
}
else
{
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", 1);
SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", 1);
SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", 1);
SYNC_DECL2(rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
ROS_FATAL("Not supposed to be here!");
}
}
}
else
{
rgbdSub_ = nh.subscribe("rgbd_image", 1, &CommonDataSubscriber::rgbdCallback, this);
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
ros::this_node::getName().c_str(),
rgbdSub_.getTopic().c_str());
}
}
bool TreeKinematics::getPositionFk(kinematics_msgs::GetPositionFK::Request& request,
kinematics_msgs::GetPositionFK::Response& response)
{
ROS_DEBUG("getPositionFK method invoked.");
KDL::JntArray q_in;
double nr_of_jnts = request.robot_state.joint_state.name.size();
q_in.resize(nr_of_jnts);
for (unsigned int i=0; i < nr_of_jnts; ++i)
{
int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]);
if (tmp_index >=0)
{
q_in(tmp_index) = request.robot_state.joint_state.position[i];
ROS_DEBUG("joint '%s' is now number '%d'", request.robot_state.joint_state.name[i].c_str(), tmp_index);
}
else
{
ROS_FATAL("i: %d, No joint index for %s!", i, request.robot_state.joint_state.name[i].c_str());
ROS_FATAL("Service call aborted.");
return false;
}
}
response.pose_stamped.resize(request.fk_link_names.size());
response.fk_link_names.resize(request.fk_link_names.size());
KDL::Frame p_out;
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
for (unsigned int i=0; i < request.fk_link_names.size(); i++)
{
ROS_DEBUG("End effector name: %s",request.fk_link_names[i].c_str());
int fk_ret = fk_solver_->JntToCart(q_in, p_out, request.fk_link_names[i]);
if (fk_ret >= 0)
{
tf_pose.frame_id_ = tree_root_name_;
tf_pose.stamp_ = ros::Time::now();
tf::PoseKDLToTF(p_out, tf_pose);
try
{
tf_listener_.transformPose(request.header.frame_id, tf_pose, tf_pose);
}
catch (tf::TransformException const &ex)
{
ROS_FATAL("Could not transform FK pose to frame: %s",request.header.frame_id.c_str());
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
tf::poseStampedTFToMsg(tf_pose, pose);
response.pose_stamped[i] = pose;
response.fk_link_names[i] = request.fk_link_names[i];
response.error_code.val = response.error_code.SUCCESS;
}
else
{
ROS_WARN("A FK solution could not be found for %s (error code = %d)",
request.fk_link_names[i].c_str(), fk_ret);
response.error_code.val = response.error_code.NO_FK_SOLUTION;
}
}
return true;
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosP3D::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
// Get the world name.
this->world_ = _parent->GetWorld();
this->model_ = _parent;
// load parameters
this->robot_namespace_ = "";
if (_sdf->HasElement("robotNamespace"))
this->robot_namespace_ =
_sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
if (!_sdf->HasElement("bodyName"))
{
ROS_FATAL("p3d plugin missing <bodyName>, cannot proceed");
return;
}
else
this->link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
this->link_ = _parent->GetLink(this->link_name_);
if (!this->link_)
{
ROS_FATAL("gazebo_ros_p3d plugin error: bodyName: %s does not exist\n",
this->link_name_.c_str());
return;
}
if (!_sdf->HasElement("topicName"))
{
ROS_FATAL("p3d plugin missing <topicName>, cannot proceed");
return;
}
else
this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();
if (!_sdf->HasElement("frameName"))
{
ROS_DEBUG("p3d plugin missing <frameName>, defaults to world");
this->frame_name_ = "world";
}
else
this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
if (!_sdf->HasElement("xyzOffset"))
{
ROS_DEBUG("p3d plugin missing <xyzOffset>, defaults to 0s");
this->offset_.pos = math::Vector3(0, 0, 0);
}
else
this->offset_.pos = _sdf->GetElement("xyzOffset")->Get<math::Vector3>();
if (!_sdf->HasElement("rpyOffset"))
{
ROS_DEBUG("p3d plugin missing <rpyOffset>, defaults to 0s");
this->offset_.rot = math::Vector3(0, 0, 0);
}
else
this->offset_.rot = _sdf->GetElement("rpyOffset")->Get<math::Vector3>();
if (!_sdf->HasElement("gaussianNoise"))
{
ROS_DEBUG("p3d plugin missing <gaussianNoise>, defaults to 0.0");
this->gaussian_noise_ = 0;
}
else
this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->Get<double>();
if (!_sdf->HasElement("updateRate"))
{
ROS_DEBUG("p3d plugin missing <updateRate>, defaults to 0.0"
" (as fast as possible)");
this->update_rate_ = 0;
}
else
this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
// publish multi queue
this->pmq.startServiceThread();
// resolve tf prefix
std::string prefix;
this->rosnode_->getParam(std::string("tf_prefix"), prefix);
this->tf_frame_name_ = tf::resolve(prefix, this->frame_name_);
if (this->topic_name_ != "")
{
this->pub_Queue = this->pmq.addPub<nav_msgs::Odometry>();
this->pub_ =
this->rosnode_->advertise<nav_msgs::Odometry>(this->topic_name_, 1);
}
this->last_time_ = this->world_->GetSimTime();
// initialize body
this->last_vpos_ = this->link_->GetWorldLinearVel();
this->last_veul_ = this->link_->GetWorldAngularVel();
this->apos_ = 0;
this->aeul_ = 0;
// if frameName specified is "/world", "world", "/map" or "map" report
// back inertial values in the gazebo world
if (this->frame_name_ != "/world" &&
this->frame_name_ != "world" &&
this->frame_name_ != "/map" &&
this->frame_name_ != "map")
{
this->reference_link_ = this->model_->GetLink(this->frame_name_);
if (!this->reference_link_)
{
ROS_ERROR("gazebo_ros_p3d plugin: frameName: %s does not exist, will"
" not publish pose\n", this->frame_name_.c_str());
return;
}
}
// init reference frame state
if (this->reference_link_)
{
ROS_DEBUG("got body %s", this->reference_link_->GetName().c_str());
this->frame_apos_ = 0;
this->frame_aeul_ = 0;
this->last_frame_vpos_ = this->reference_link_->GetWorldLinearVel();
this->last_frame_veul_ = this->reference_link_->GetWorldAngularVel();
}
// start custom queue for p3d
this->callback_queue_thread_ = boost::thread(
boost::bind(&GazeboRosP3D::P3DQueueThread, this));
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosP3D::UpdateChild, this));
}
bool TreeKinematics::getPositionIk(tree_kinematics::get_tree_position_ik::Request &request,
tree_kinematics::get_tree_position_ik::Response &response)
{
ik_srv_duration_ = ros::Time::now().toSec();
ROS_DEBUG("getPositionIK method invoked.");
// extract current joint positions from the request
KDL::JntArray q, q_desi;
double nr_of_jnts = request.pos_ik_request[0].ik_seed_state.joint_state.name.size();
q.resize(nr_of_jnts);
q_desi.resize(nr_of_jnts);
for (unsigned int i=0; i < nr_of_jnts; ++i)
{
int tmp_index = getJointIndex(request.pos_ik_request[0].ik_seed_state.joint_state.name[i]);
if (tmp_index >=0)
{
q(tmp_index) = request.pos_ik_request[0].ik_seed_state.joint_state.position[i];
ROS_DEBUG("joint '%s' is now number '%d'", request.pos_ik_request[0].ik_seed_state.joint_state.name[i].c_str(),
tmp_index);
}
else
{
ROS_FATAL("i: %d, No joint index for %s!",i,request.pos_ik_request[0].ik_seed_state.joint_state.name[i].c_str());
ROS_FATAL("Service call aborted.");
return false;
}
}
// convert pose messages into transforms from the root frame to the given poses and further into KDL::Frames
geometry_msgs::PoseStamped pose_msg_in;
geometry_msgs::PoseStamped pose_msg_transformed;
tf::Stamped<tf::Pose> transform;
tf::Stamped<tf::Pose> transform_root;
KDL::Frames desired_poses;
KDL::Frame desired_pose;
for(unsigned int i = 0; i < request.pos_ik_request.size(); ++i)
{
pose_msg_in = request.pos_ik_request[i].pose_stamped;
try
{
tf_listener_.waitForTransform(tree_root_name_, pose_msg_in.header.frame_id, pose_msg_in.header.stamp,
ros::Duration(0.1));
tf_listener_.transformPose(tree_root_name_, pose_msg_in, pose_msg_transformed);
}
catch (tf::TransformException const &ex)
{
ROS_FATAL("%s",ex.what());
ROS_FATAL("Could not transform IK pose from '%s' to frame '%s'", tree_root_name_.c_str(),
pose_msg_in.header.frame_id.c_str());
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
tf::PoseMsgToKDL(pose_msg_transformed.pose, desired_pose);
desired_poses[request.pos_ik_request[i].ik_link_name] = desired_pose;
}
// use the solver to compute desired joint positions
ik_duration_ = ros::Time::now().toSec();
int ik_ret = ik_pos_solver_->CartToJnt_it(q, desired_poses, q_desi); // NOTE: Before it was CartToJnt (without the _it). What's the difference?
ik_duration_ = ros::Time::now().toSec() - ik_duration_;
ik_duration_median_ = ((ik_duration_median_ * (loop_count_ - 1)) + ik_duration_) / loop_count_;
// insert the solver's result into the service response
if (ik_ret >= 0 || ik_ret == -3)
{
response.solution.joint_state.name = info_.joint_names;
response.solution.joint_state.position.resize(nr_of_jnts_);
response.solution.joint_state.velocity.resize(nr_of_jnts_);
for (unsigned int i=0; i < nr_of_jnts_; ++i)
{
response.solution.joint_state.position[i] = q_desi(i);
response.solution.joint_state.velocity[i] = (q_desi(i) - q(i)) * srv_call_frequency_;
ROS_DEBUG("IK Solution: %s %d: pos = %f, vel = %f",response.solution.joint_state.name[i].c_str(), i,
response.solution.joint_state.position[i], response.solution.joint_state.velocity[i]);
}
response.error_code.val = response.error_code.SUCCESS;
ik_srv_duration_ = ros::Time::now().toSec() - ik_srv_duration_;
ik_srv_duration_median_ = ((ik_srv_duration_median_ * (loop_count_ - 1)) + ik_srv_duration_) / loop_count_;
loop_count_++;
ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_srv_duration %f and median %f", ik_srv_duration_,
ik_srv_duration_median_);
ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_duration %f and median %f", ik_duration_, ik_duration_median_);
if (ik_ret == -3)
{
ROS_WARN_THROTTLE(1.0, "Maximum iterations reached! (error code = %d)", ik_ret);
}
}
else
{
ROS_WARN("An IK solution could not be found (error code = %d)", ik_ret);
response.error_code.val = response.error_code.NO_IK_SOLUTION;
ik_srv_duration_ = ros::Time::now().toSec() - ik_srv_duration_;
ik_srv_duration_median_ = ((ik_srv_duration_median_ * (loop_count_ - 1)) + ik_srv_duration_) / loop_count_;
loop_count_++;
ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_srv_duration %f and median %f", ik_srv_duration_,
ik_srv_duration_median_);
ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_duration %f and median %f", ik_duration_, ik_duration_median_);
}
return true;
}
int main(int argc, char** argv)
{
ROS_INFO("Started fullbody_teleop.");
ros::init(argc, argv, "fullbody_teleop");
ros::NodeHandle nh;
std::string robotDescription;
if (!nh.getParam("/robot_description", robotDescription))
{
ROS_FATAL("Parameter for robot description not provided");
}
huboModel.initString(robotDescription);
//ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);
gIntServer.reset( new interactive_markers::InteractiveMarkerServer("joint_controls","",false) );
for (auto jointPair : huboModel.joints_)
{
boost::shared_ptr<urdf::Joint> joint = jointPair.second;
if (joint->name[1] == 'F') { continue; }
if (joint->name== "TSY") { continue; }
makeJointMarker(joint->name);
}
std::cerr << "\nURDF size: " << huboModel.joints_.size() << std::endl;
ros::Duration(0.1).sleep();
for (int i = 0; i < HUBO_JOINT_COUNT; i++)
{
if (DRCHUBO_URDF_JOINT_NAMES[i] != "")
{
planState.name.push_back(DRCHUBO_URDF_JOINT_NAMES[i]);
planState.position.push_back(0);
planState.velocity.push_back(0);
planState.effort.push_back(0);
}
}
for (int side = 0; side < 2; side++)
for (int i = 1; i <= 3; i++)
for (int j = 1; j <= 3; j++)
{
std::string sideStr = (side == 0) ? "R" : "L";
planState.name.push_back(sideStr + "F" + std::to_string(i) + std::to_string(j));
planState.position.push_back(0);
planState.velocity.push_back(0);
planState.effort.push_back(0);
}
makeSaveButton();
gIntServer->applyChanges();
gJoySubscriber = nh.subscribe("joy_in", 1, &joyCallback);
gIKinClient = nh.serviceClient<moveit_msgs::GetPositionIK>("/hubo/kinematics/ik_service");
gFKinClient = nh.serviceClient<moveit_msgs::GetPositionFK>("/hubo/kinematics/fk_service");
gStatePublisher = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
gRPosePublisher = nh.advertise<geometry_msgs::PoseStamped>("rh_pose", 1);
gTextPublisher = nh.advertise<std_msgs::String>("text_out", 1);
gTimer = nh.createTimer(ros::Duration(1), &timerCallback);
gTimer.start();
ros::spin();
gIntServer.reset();
return 0;
}
ImuFilter::ImuFilter(ros::NodeHandle nh, ros::NodeHandle nh_private):
nh_(nh),
nh_private_(nh_private),
initialized_(false),
q0(1.0), q1(0.0), q2(0.0), q3(0.0)
{
ROS_INFO ("Starting ImuFilter");
// **** get paramters
if (!nh_private_.getParam ("gain", gain_))
gain_ = 0.1;
if (!nh_private_.getParam ("use_mag", use_mag_))
use_mag_ = true;
if (!nh_private_.getParam ("publish_tf", publish_tf_))
publish_tf_ = true;
if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
fixed_frame_ = "odom";
if (!nh_private_.getParam ("constant_dt", constant_dt_))
constant_dt_ = 0.0;
// check for illegal constant_dt values
if (constant_dt_ < 0.0)
{
ROS_FATAL("constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", constant_dt_);
constant_dt_ = 0.0;
}
// if constant_dt_ is 0.0 (default), use IMU timestamp to determine dt
// otherwise, it will be constant
if (constant_dt_ == 0.0)
ROS_INFO("Using dt computed from message headers");
else
ROS_INFO("Using constant dt of %f sec", constant_dt_);
// **** register dynamic reconfigure
FilterConfigServer::CallbackType f = boost::bind(&ImuFilter::reconfigCallback, this, _1, _2);
config_server_.setCallback(f);
// **** register publishers
imu_publisher_ = nh_.advertise<sensor_msgs::Imu>(
"imu/data", 5);
// **** register subscribers
// Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
int queue_size = 5;
imu_subscriber_.reset(new ImuSubscriber(
nh_, "imu/data_raw", queue_size));
if (use_mag_)
{
mag_subscriber_.reset(new MagSubscriber(
nh_, "imu/mag", queue_size));
sync_.reset(new Synchronizer(
SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_));
sync_->registerCallback(boost::bind(&ImuFilter::imuMagCallback, this, _1, _2));
}
else
{
imu_subscriber_->registerCallback(&ImuFilter::imuCallback, this);
}
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
world = _model->GetWorld();
// load parameters
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
else
namespace_.clear();
if (!_sdf->HasElement("topicName"))
topic_ = "magnetic";
else
topic_ = _sdf->GetElement("topicName")->Get<std::string>();
if (_sdf->HasElement("bodyName"))
{
link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
link = _model->GetLink(link_name_);
}
else
{
link = _model->GetLink();
link_name_ = link->GetName();
}
if (!link)
{
ROS_FATAL("GazeboRosMagnetic plugin error: bodyName: %s does not exist\n", link_name_.c_str());
return;
}
// default parameters
frame_id_ = link_name_;
magnitude_ = DEFAULT_MAGNITUDE;
reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
declination_ = DEFAULT_DECLINATION * M_PI/180.0;
inclination_ = DEFAULT_INCLINATION * M_PI/180.0;
if (_sdf->HasElement("frameId"))
frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();
if (_sdf->HasElement("magnitude"))
_sdf->GetElement("magnitude")->GetValue()->Get(magnitude_);
if (_sdf->HasElement("referenceHeading"))
if (_sdf->GetElement("referenceHeading")->GetValue()->Get(reference_heading_))
reference_heading_ *= M_PI/180.0;
if (_sdf->HasElement("declination"))
if (_sdf->GetElement("declination")->GetValue()->Get(declination_))
declination_ *= M_PI/180.0;
if (_sdf->HasElement("inclination"))
if (_sdf->GetElement("inclination")->GetValue()->Get(inclination_))
inclination_ *= M_PI/180.0;
// Note: Gazebo uses NorthWestUp coordinate system, heading and declination are compass headings
magnetic_field_.header.frame_id = frame_id_;
magnetic_field_world_.x = magnitude_ * cos(inclination_) * cos(reference_heading_ - declination_);
magnetic_field_world_.y = magnitude_ * cos(inclination_) * sin(reference_heading_ - declination_);
magnetic_field_world_.z = magnitude_ * -sin(inclination_);
sensor_model_.Load(_sdf);
// start ros node
if (!ros::isInitialized())
{
int argc = 0;
char** argv = NULL;
ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
}
node_handle_ = new ros::NodeHandle(namespace_);
publisher_ = node_handle_->advertise<geometry_msgs::Vector3Stamped>(topic_, 1);
// setup dynamic_reconfigure server
dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, topic_)));
dynamic_reconfigure_server_->setCallback(boost::bind(&SensorModel3::dynamicReconfigureCallback, &sensor_model_, _1, _2));
Reset();
// connect Update function
updateTimer.Load(world, _sdf);
updateConnection = updateTimer.Connect(boost::bind(&GazeboRosMagnetic::Update, this));
}
void cb_points(const visualization_msgs::MarkerConstPtr& data)
{
bool success = false;
double x = data->pose.position.x;
double y = data->pose.position.y ;
double z = data->pose.position.z;
ROS_INFO("Points received! x: %f y: %f z: %f",x,y,z);
moveit::planning_interface::MoveGroup group("manipulator");
group.setPlanningTime(45.0);
group.setNumPlanningAttempts(10);
group.allowReplanning(true);
group.clearPathConstraints();
co.header.stamp = ros::Time::now();
co.header.frame_id = "base_link";
co.id = "testbox";
co.primitives.resize(1);
co.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
co.primitives[0].dimensions.resize(shape_tools::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::CYLINDER>::value);
co.primitives[0].dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = 0.04;
co.primitives[0].dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = 0.14;
//co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.14;
co.primitive_poses.resize(1);
co.primitive_poses[0].position.x = x;
co.primitive_poses[0].position.y = y;
co.primitive_poses[0].position.z = z;
co.primitive_poses[0].orientation.w = 1.0;
aco.object = co;
aco.link_name = "katana_gripper_tool_frame";
add_collision_object();
ros::WallDuration(5.0).sleep();
ROS_INFO("Generating Grasps");
std::vector<moveit_msgs::Grasp> grasps = generate_grasps(x, y, z);
success = group.pick("testbox",grasps);
if (success)
{
ROS_INFO("Pick was successful.");
}
else
{
ROS_FATAL("Pick failed.");
//return EXIT_FAILURE;
}
ros::WallDuration(3.0).sleep();
if(success)
{
std::vector<moveit_msgs::PlaceLocation> places = generate_places(x, -y, z);
success = group.place("testbox",places);
if (success)
{
ROS_INFO("Place was successful.");
geometry_msgs::PoseStamped po = group.getCurrentPose();
ROS_INFO("x: %f y: %f z: %f",po.pose.position.x,po.pose.position.y,po.pose.position.z);
ROS_INFO("w: %f, x: %f y: %f z: %f",po.pose.orientation.w, po.pose.orientation.x, po.pose.orientation.y, po.pose.orientation.z);
}
else
{
ROS_FATAL("Place failed.");
//return EXIT_FAILURE;
}
}
group.setStartState(*group.getCurrentState());
remove_collision_object();
group.setStartState(*group.getCurrentState());
group.setNamedTarget("home");
group.move();
geometry_msgs::PoseStamped po = group.getCurrentPose();
ROS_INFO("x: %f y: %f z: %f",po.pose.position.x,po.pose.position.y,po.pose.position.z);
ROS_INFO("w: %f, x: %f y: %f z: %f",po.pose.orientation.w, po.pose.orientation.x, po.pose.orientation.y, po.pose.orientation.z);
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
world = _model->GetWorld();
// load parameters
if (!_sdf->HasElement("robotNamespace"))
namespace_.clear();
else
namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
if (!_sdf->HasElement("topicName"))
topic_ = "magnetic";
else
topic_ = _sdf->GetElement("topicName")->GetValueString();
if (!_sdf->HasElement("bodyName"))
{
link = _model->GetLink();
link_name_ = link->GetName();
}
else {
link_name_ = _sdf->GetElement("bodyName")->GetValueString();
link = boost::shared_dynamic_cast<physics::Link>(world->GetEntity(link_name_));
}
if (!link)
{
ROS_FATAL("GazeboRosMagnetic plugin error: bodyName: %s does not exist\n", link_name_.c_str());
return;
}
double update_rate = 0.0;
if (_sdf->HasElement("updateRate")) update_rate = _sdf->GetElement("updateRate")->GetValueDouble();
update_period = update_rate > 0.0 ? 1.0/update_rate : 0.0;
if (!_sdf->HasElement("frameId"))
frame_id_ = link_name_;
else
frame_id_ = _sdf->GetElement("frameId")->GetValueString();
if (!_sdf->HasElement("magnitude"))
magnitude_ = DEFAULT_MAGNITUDE;
else
magnitude_ = _sdf->GetElement("magnitude")->GetValueDouble();
if (!_sdf->HasElement("referenceHeading"))
reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
else
reference_heading_ = _sdf->GetElement("referenceHeading")->GetValueDouble() * M_PI/180.0;
if (!_sdf->HasElement("declination"))
declination_ = DEFAULT_DECLINATION * M_PI/180.0;
else
declination_ = _sdf->GetElement("declination")->GetValueDouble() * M_PI/180.0;
if (!_sdf->HasElement("inclination"))
inclination_ = DEFAULT_INCLINATION * M_PI/180.0;
else
inclination_ = _sdf->GetElement("inclination")->GetValueDouble() * M_PI/180.0;
// Note: Gazebo uses NorthWestUp coordinate system, heading and declination are compass headings
magnetic_field_.header.frame_id = frame_id_;
magnetic_field_world_.x = magnitude_ * cos(inclination_) * cos(reference_heading_ - declination_);
magnetic_field_world_.y = magnitude_ * sin(reference_heading_ - declination_);
magnetic_field_world_.z = magnitude_ * -sin(inclination_) * cos(reference_heading_ - declination_);
sensor_model_.Load(_sdf);
// start ros node
if (!ros::isInitialized())
{
int argc = 0;
char** argv = NULL;
ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
}
node_handle_ = new ros::NodeHandle(namespace_);
publisher_ = node_handle_->advertise<geometry_msgs::Vector3Stamped>(topic_, 1);
Reset();
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosMagnetic::Update, this));
}
////////////////////////////////////////////////////////////////////////////////
// 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;
}
}
UsbCamNode() :
node_("~")
{
image_transport::ImageTransport it(node_);
std::string topicName;
node_.param("topicName",topicName,std::string("image_raw"));
image_pub_ = it.advertiseCamera(topicName, 1);
node_.param("video_device", video_device_name_, std::string("/dev/video0"));
node_.param("io_method", io_method_name_, std::string("mmap")); // possible values: mmap, read, userptr
node_.param("image_width", image_width_, 640);
node_.param("image_height", image_height_, 480);
node_.param("framerate", framerate_, 30);
node_.param("pixel_format", pixel_format_name_, std::string("mjpeg")); // possible values: yuyv, uyvy, mjpeg
node_.param("autofocus", autofocus_, false); // enable/disable autofocus
node_.param("camera_frame_id", img_.header.frame_id, std::string("head_camera"));
node_.param("camera_name", camera_name_, std::string("head_camera"));
node_.param("camera_info_url", camera_info_url_, std::string(""));
cinfo_.reset(new camera_info_manager::CameraInfoManager(node_, camera_name_, camera_info_url_));
ROS_INFO("Camera name: %s", camera_name_.c_str());
ROS_INFO("Camera info url: %s", camera_info_url_.c_str());
ROS_INFO("usb_cam video_device set to [%s]\n", video_device_name_.c_str());
ROS_INFO("usb_cam io_method set to [%s]\n", io_method_name_.c_str());
ROS_INFO("usb_cam image_width set to [%d]\n", image_width_);
ROS_INFO("usb_cam image_height set to [%d]\n", image_height_);
ROS_INFO("usb_cam pixel_format set to [%s]\n", pixel_format_name_.c_str());
ROS_INFO("usb_cam auto_focus set to [%d]\n", autofocus_);
usb_cam_io_method io_method;
if(io_method_name_ == "mmap")
io_method = IO_METHOD_MMAP;
else if(io_method_name_ == "read")
io_method = IO_METHOD_READ;
else if(io_method_name_ == "userptr")
io_method = IO_METHOD_USERPTR;
else {
ROS_FATAL("Unknown io method.");
node_.shutdown();
return;
}
usb_cam_pixel_format pixel_format;
if(pixel_format_name_ == "yuyv")
pixel_format = PIXEL_FORMAT_YUYV;
else if(pixel_format_name_ == "uyvy")
pixel_format = PIXEL_FORMAT_UYVY;
else if(pixel_format_name_ == "mjpeg") {
pixel_format = PIXEL_FORMAT_MJPEG;
}
else {
ROS_FATAL("Unknown pixel format.");
node_.shutdown();
return;
}
camera_image_ = usb_cam_camera_start(video_device_name_.c_str(),
io_method,
pixel_format,
image_width_,
image_height_,
framerate_);
if(autofocus_) {
usb_cam_camera_set_auto_focus(1);
}
next_time_ = ros::Time::now();
count_ = 0;
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosIMU::LoadThread()
{
// load parameters
this->robot_namespace_ = "";
if (this->sdf->HasElement("robotNamespace"))
this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";
if (!this->sdf->HasElement("serviceName"))
{
ROS_INFO("imu plugin missing <serviceName>, defaults to /default_imu");
this->service_name_ = "/default_imu";
}
else
this->service_name_ = this->sdf->Get<std::string>("serviceName");
if (!this->sdf->HasElement("topicName"))
{
ROS_INFO("imu plugin missing <topicName>, defaults to /default_imu");
this->topic_name_ = "/default_imu";
}
else
this->topic_name_ = this->sdf->Get<std::string>("topicName");
if (!this->sdf->HasElement("gaussianNoise"))
{
ROS_INFO("imu plugin missing <gaussianNoise>, defaults to 0.0");
this->gaussian_noise_ = 0.0;
}
else
this->gaussian_noise_ = this->sdf->Get<double>("gaussianNoise");
if (!this->sdf->HasElement("bodyName"))
{
ROS_FATAL("imu plugin missing <bodyName>, cannot proceed");
return;
}
else
this->link_name_ = this->sdf->Get<std::string>("bodyName");
if (!this->sdf->HasElement("xyzOffset"))
{
ROS_INFO("imu plugin missing <xyzOffset>, defaults to 0s");
this->offset_.pos = math::Vector3(0, 0, 0);
}
else
this->offset_.pos = this->sdf->Get<math::Vector3>("xyzOffset");
if (!this->sdf->HasElement("rpyOffset"))
{
ROS_INFO("imu plugin missing <rpyOffset>, defaults to 0s");
this->offset_.rot = math::Vector3(0, 0, 0);
}
else
this->offset_.rot = this->sdf->Get<math::Vector3>("rpyOffset");
if (!this->sdf->HasElement("updateRate"))
{
ROS_DEBUG("imu plugin missing <updateRate>, defaults to 0.0"
" (as fast as possible)");
this->update_rate_ = 0.0;
}
else
this->update_rate_ = this->sdf->GetElement("updateRate")->Get<double>();
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
// publish multi queue
this->pmq.startServiceThread();
// assert that the body by link_name_ exists
this->link = boost::dynamic_pointer_cast<physics::Link>(
this->world_->GetEntity(this->link_name_));
if (!this->link)
{
ROS_FATAL("gazebo_ros_imu plugin error: bodyName: %s does not exist\n",
this->link_name_.c_str());
return;
}
// if topic name specified as empty, do not publish
if (this->topic_name_ != "")
{
this->pub_Queue = this->pmq.addPub<sensor_msgs::Imu>();
this->pub_ = this->rosnode_->advertise<sensor_msgs::Imu>(
this->topic_name_, 1);
// advertise services on the custom queue
ros::AdvertiseServiceOptions aso =
ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
this->service_name_, boost::bind(&GazeboRosIMU::ServiceCallback,
this, _1, _2), ros::VoidPtr(), &this->imu_queue_);
this->srv_ = this->rosnode_->advertiseService(aso);
}
// Initialize the controller
this->last_time_ = this->world_->GetSimTime();
// this->initial_pose_ = this->link->GetPose();
this->last_vpos_ = this->link->GetWorldLinearVel();
this->last_veul_ = this->link->GetWorldAngularVel();
this->apos_ = 0;
this->aeul_ = 0;
// start custom queue for imu
this->callback_queue_thread_ =
boost::thread(boost::bind(&GazeboRosIMU::IMUQueueThread, this));
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosIMU::UpdateChild, this));
}
void callback(const sensor_msgs::JointState::ConstPtr& jointStatePtr)
{
ROS_INFO_STREAM("I'm in the callback");
float L1 = 0.3;
float L2 = 0.31;
float theta1_L, theta1_R, theta2_L, theta2_R;
float mPError1, mPError2;
float r = sqrt(pow(x_estimate, 2)+pow(y_estimate, 2));
float alpha = acosf((powf(L1,2)+powf(L2,2)-powf(r,2))/(2.0*L1*L2));
float beta = acosf((powf(L1,2)-powf(L2,2)+powf(r,2))/(2.0*L1*r));
theta2_L = M_PI + alpha;
theta2_R = M_PI - alpha;
theta1_L = atan2f(y_estimate, x_estimate) + beta;
theta1_R = atan2f(y_estimate, x_estimate) - beta;
theta1_L = wrapAngle(theta1_L);
theta1_R = wrapAngle(theta1_R);
theta2_L = wrapAngle(theta2_L);
theta2_R = wrapAngle(theta2_R);
sensor_msgs::JointState output;
output.header.stamp = ros::Time::now();
output.header.frame_id = "";
if (direction == "left")
{
mPError1 = theta1_L-jointStatePtr->position[0];
//mIError1 += theta1_L-jointStatePtr->position[0];
mPError2 = theta2_L-jointStatePtr->position[2];
//mIError2 += theta2_L-jointStatePtr->position[2];
}
else if (direction == "right")
{
mPError1 = theta1_R-jointStatePtr->position[0];
mPError2 = theta2_R-jointStatePtr->position[2];
}
else
ROS_FATAL("COMMAND WAS NEITHER LEFT NOR RIGHT!!!");
ROS_INFO("******************************GOING FOR IT!!*****************************************\n");
ROS_INFO("x_board=%f, y_board=%f \n", x_estimate, y_estimate);
ROS_INFO("r = %f, alpha = %f, beta = %f\n", r, alpha, beta);
ROS_INFO("theta1_L = %f, theta1_R = %f, theta2_L = %f, theta2_R = %f\n", theta1_L, theta1_R, theta2_L, theta2_R);
ROS_INFO("Joint Positions: (%f, %f, %f)\n", jointStatePtr->position[0], jointStatePtr->position[1], jointStatePtr->position[2]);
ROS_INFO("P Error_1 = %f, P Error_2 = %f\n", mPError1, mPError2);
ROS_INFO("***********************************************************************\n");
//ROS_INFO_STREAM("Back in main, global counter is: " << globalcounter);
float gain=0.2;
float command1 = jointStatePtr->position[0]+gain*mPError1;
float command2 = jointStatePtr->position[2]+gain*mPError2;
ROS_INFO_STREAM("Command to motor 21: " << command1 << " Command to motor 23: " << command2);
//std::cout << "commanding: " << x_estimate << ", " << y_estimate << std::endl;
if ((finalMoveFlag == 1) && (idleFlag ==0))
{
ROS_INFO("finalMoveFlag is 1");
output.position.push_back(jointStatePtr->position[0]+mPError1);
output.position.push_back(1.0);
output.position.push_back(jointStatePtr->position[2]+mPError2);
}
else if (idleFlag == 0)
{
output.position.push_back(command1);
output.position.push_back(1.0);
output.position.push_back(command2);
}
else
{
ROS_INFO("Idling the motors");
output.position.push_back(NAN);
output.position.push_back(1.0);
output.position.push_back(NAN);
output.velocity.push_back(0);
output.velocity.push_back(0);
output.velocity.push_back(0);
}
//.... do something with the input and generate the output...
pub_.publish(output);
ROS_INFO_STREAM("Just published!");
}
int main( int argc, char** argv ) {
// initialize ROS
ros::init(argc, argv, "iiwa_hw", ros::init_options::NoSigintHandler);
// ros spinner
ros::AsyncSpinner spinner(1);
spinner.start();
// custom signal handlers
signal(SIGTERM, quitRequested);
signal(SIGINT, quitRequested);
signal(SIGHUP, quitRequested);
// construct the lbr iiwa
ros::NodeHandle iiwa_nh;
IIWA_HW iiwa_robot(iiwa_nh);
// configuration routines
iiwa_robot.start();
// timer variables
struct timespec ts = {0, 0};
ros::Time last(ts.tv_sec, ts.tv_nsec), now(ts.tv_sec, ts.tv_nsec);
ros::Duration period(1.0);
//the controller manager
controller_manager::ControllerManager manager(&iiwa_robot, iiwa_nh);
// run as fast as possible
while( !g_quit ) {
// get the time / period
if (!clock_gettime(CLOCK_REALTIME, &ts)) {
now.sec = ts.tv_sec;
now.nsec = ts.tv_nsec;
period = now - last;
last = now;
} else {
ROS_FATAL("Failed to poll realtime clock!");
break;
}
// read current robot position
iiwa_robot.read(period);
// update the controllers
manager.update(now, period);
// send command position to the robot
iiwa_robot.write(period);
// wait for some milliseconds defined in controlFrequency
iiwa_robot.getRate()->sleep();
}
std::cerr << "Stopping spinner..." << std::endl;
spinner.stop();
std::cerr << "Bye!" << std::endl;
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "miniq_pid_server");
ros::NodeHandle n;
ros::NodeHandle pn("~");
std::string port;
pn.param<std::string>("port", port, "/dev/ttyUSB0");
int baudrate;
pn.param("baudrate", baudrate, 57600);
ros::Publisher left_pub = n.advertise<lse_miniq_msgs::WheelVelocity>("/left_wheel_velocity", 20);
ros::Publisher right_pub = n.advertise<lse_miniq_msgs::WheelVelocity>("/right_wheel_velocity", 20);
if(!miniQ::openPort((char*)port.c_str(), baudrate))
{
ROS_FATAL("miniQ -- Failed to open serial port %s at %d baud!", port.c_str(), baudrate);
ROS_BREAK();
}
ROS_INFO("miniQ PID Server -- Successfully connected to the miniQ!");
ros::Duration(3.0).sleep();
if(!robot.checkVersion())
{
ROS_FATAL("miniQ PID Server -- The firmware version of the miniQ robot is not compatible with this ROS node!");
ROS_BREAK();
}
dynamic_reconfigure::Server<lse_miniq_driver::miniQConfig> server;
dynamic_reconfigure::Server<lse_miniq_driver::miniQConfig>::CallbackType f;
f = boost::bind(&callback, _1, _2);
server.setCallback(f);
turn_robot_off = false;
ros::Rate r(2.5);
while(n.ok())
{
if(!robot.updateWheelVelocities()) ROS_ERROR("miniQ PID Server -- Failed to update the miniQ wheel velocities!!!");
else
{
lse_miniq_msgs::WheelVelocity right_msg;
right_msg.reference = wheel_velocity_reference;
right_msg.measured = robot.getRightWheelVelocity();
right_pub.publish(right_msg);
lse_miniq_msgs::WheelVelocity left_msg;
left_msg.reference = wheel_velocity_reference;
left_msg.measured = robot.getLeftWheelVelocity();
left_pub.publish(left_msg);
}
if(l_s_updated)
{
l_s_updated = false;
if(!robot.setPIDGains(l_s_kp, l_s_ki, l_s_kd, miniQ::LeftStartingPID)) ROS_ERROR("miniQ PID Server -- ERROR updating left starting PID!");
ros::Duration(0.1).sleep();
}
if(l_r_updated)
{
l_r_updated = false;
if(!robot.setPIDGains(l_r_kp, l_r_ki, l_r_kd, miniQ::LeftRunningPID)) ROS_ERROR("miniQ PID Server -- ERROR updating left running PID!");
ros::Duration(0.1).sleep();
}
if(r_s_updated)
{
r_s_updated = false;
if(!robot.setPIDGains(r_s_kp, r_s_ki, r_s_kd, miniQ::RightStartingPID)) ROS_ERROR("miniQ PID Server -- ERROR updating right starting PID!");
ros::Duration(0.1).sleep();
}
if(r_r_updated)
{
r_r_updated = false;
if(!robot.setPIDGains(r_r_kp, r_r_ki, r_r_kd, miniQ::RightRunningPID)) ROS_ERROR("miniQ PID Server -- ERROR updating right running PID!");
ros::Duration(0.1).sleep();
}
if(turn_robot_off)
{
turn_robot_off = false;
robot.setVelocities(0.0, 0.0);
}
else if(run_robot && wheel_velocity_reference_updated)
{
wheel_velocity_reference_updated = false;
robot.setVelocities(wheel_velocity_reference, 0.0);
robot.updateVelocities();
}
ros::spinOnce();
r.sleep();
}
return 0;
}
void Main::process()
{
if(autoFaceDetection && !face_cascade.load(face_cascade_path))
{
ROS_FATAL("--(!)Error loading cascade detector\n");
return;
}
while (ros::ok())
{
switch (state)
{
case INIT:
if(newImageReceived())
{
if(showOutput)
sendObjectTracked(0, 0, 0, 0, 0);
getLastImageFromBuffer();
tld->detectorCascade->imgWidth = gray.cols;
tld->detectorCascade->imgHeight = gray.rows;
tld->detectorCascade->imgWidthStep = gray.step;
state = TRACKER_INIT;
}
break;
case TRACKER_INIT:
if(loadModel && !modelImportFile.empty())
{
ROS_INFO("Loading model %s", modelImportFile.c_str());
tld->readFromFile(modelImportFile.c_str());
tld->learningEnabled = false;
state = TRACKING;
}
else if(autoFaceDetection || correctBB)
{
if(autoFaceDetection)
{
target_image = gray;
target_bb = faceDetection();
}
sendObjectTracked(target_bb.x,target_bb.y,target_bb.width,target_bb.height,1.0);
ROS_INFO("Starting at %d %d %d %d\n", target_bb.x, target_bb.y, target_bb.width, target_bb.height);
tld->selectObject(target_image, &target_bb);
tld->learningEnabled = true;
state = TRACKING;
}
else
{
ros::Duration(1.0).sleep();
ROS_INFO("Waiting for a BB");
}
break;
case TRACKING:
if(newImageReceived())
{
ros::Time tic = ros::Time::now();
getLastImageFromBuffer();
tld->processImage(img);
ros::Duration toc = (ros::Time::now() - tic);
float fps = 1.0/toc.toSec();
std_msgs::Float32 msg_fps;
msg_fps.data = fps;
pub2.publish(msg_fps);
if(showOutput && tld->currBB != NULL)
{
sendObjectTracked(tld->currBB->x,tld->currBB->y,tld->currBB->width,tld->currBB->height,tld->currConf);
}
}
break;
case STOPPED:
ros::Duration(1.0).sleep();
ROS_INFO("Tracker stopped");
break;
default:
break;
}
}
if(exportModelAfterRun)
{
tld->writeToFile(modelExportFile.c_str());
}
semaphore.unlock();
}