int main(int argc, char** argv)
{
ros::init(argc, argv, "SHORT_NAME");
ROS_INFO_STREAM_NAMED("main", "Starting CLASS_NAME...");
// Allow the action server to recieve and send ros messages
ros::AsyncSpinner spinner(2);
spinner.start();
// Check for verbose flag
bool verbose = false;
if (argc > 1)
{
for (int i = 0; i < argc; ++i)
{
if (strcmp(argv[i], "--verbose") == 0)
{
ROS_INFO_STREAM_NAMED("main","Running in VERBOSE mode (slower)");
verbose = true;
continue;
}
}
}
PACKAGE_NAME::CLASS_NAME server();
ROS_INFO_STREAM_NAMED("main", "Shutting down.");
ros::shutdown();
return 0;
}
bool Action::graspPlan(MetaBlock *block, const std::string surface_name) //computePlanButtonClicked
{
bool success(false);
if (verbose_)
ROS_INFO_STREAM_NAMED("pick_place:","Planning " << block->name << " at pose " << block->start_pose);
move_group_->setGoalTolerance(0.1);//0.05 //TODO
// Prevent collision with table
if (!surface_name.empty())
move_group_->setSupportSurfaceName(surface_name);
if (!move_group_)
return false;
//move_group_->setApproximateJointValueTargets(target, move_group_->getEndEffectorLink().c_str());
move_group_->setPoseTargets(configureForPlanning(generateGrasps(block)), move_group_->getEndEffectorLink().c_str());
current_plan_.reset(new moveit::planning_interface::MoveGroup::Plan());
success = move_group_->plan(*current_plan_);
if (!success)
current_plan_.reset();
if (verbose_ && success)
ROS_INFO_STREAM_NAMED("pick_place","Grasp plannin success! \n\n");
return success;
}
// Execute series of tasks for pick/place
bool pickAndPlace(const geometry_msgs::Pose& start_block_pose, const geometry_msgs::Pose& end_block_pose)
{
ROS_INFO_STREAM_NAMED("pick_place","Pick and place started");
// ---------------------------------------------------------------------------------------------
// Visualize the two blocks
rviz_tools_->publishBlock(start_block_pose);
rviz_tools_->publishBlock(end_block_pose);
// ---------------------------------------------------------------------------------------------
// Generate graps
ROS_INFO_STREAM_NAMED("pick_place_moveit","Generating grasps for pick and place");
bool rviz_verbose = true;
moveit_simple_grasps::SimpleGrasps grasp_generator(rviz_tools_);
// Pick grasp
std::vector<moveit_msgs::Grasp> possible_grasps;
grasp_generator.generateBlockGrasps( start_block_pose, grasp_data_, possible_grasps );
// Filter grasp poses
//moveit_simple_grasps::GraspFilter grasp_filter( planning_scene_monitor_->getPlanningScene()->getCurrentState() ...
//if( !grasp_generator.filterGrasps( possible_grasps ) )
//return false;
// Send pick command to move_group
executeGrasps(possible_grasps, start_block_pose);
return true;
}
// Execute series of tasks for pick/place
bool pickAndPlace(const geometry_msgs::Pose& start_block_pose, const geometry_msgs::Pose& end_block_pose)
{
ROS_INFO_STREAM_NAMED("pick_place","Pick and place started");
// ---------------------------------------------------------------------------------------------
// Visualize the two blocks
rviz_tools_->publishBlock(start_block_pose, BLOCK_SIZE, true);
rviz_tools_->publishBlock(end_block_pose, BLOCK_SIZE, false);
// ---------------------------------------------------------------------------------------------
// Generate graps
ROS_INFO_STREAM_NAMED("pick_place_moveit","Generating grasps for pick and place");
bool rviz_verbose = true;
block_grasp_generator::GraspGenerator grasp_generator( base_link_, PLANNING_GROUP_NAME, rviz_tools_, rviz_verbose);
// Pick grasp
std::vector<manipulation_msgs::Grasp> possible_grasps;
grasp_generator.generateGrasps( start_block_pose, possible_grasps );
// Filter grasp poses
//block_grasp_generator::GraspFilter grasp_filter( planning_scene_monitor_->getPlanningScene()->getCurrentState() ...
//if( !grasp_generator.filterGrasps( possible_grasps ) )
//return false;
// Send pick command to move_group
executeGrasps(possible_grasps, start_block_pose);
return true;
}
void GenericHWInterface::loadURDF(ros::NodeHandle &nh, std::string param_name)
{
std::string urdf_string;
urdf_model_ = new urdf::Model();
// search and wait for robot_description on param server
while (urdf_string.empty() && ros::ok())
{
std::string search_param_name;
if (nh.searchParam(param_name, search_param_name))
{
ROS_INFO_STREAM_NAMED("generic_hw_interface", "Waiting for model URDF on the ROS param server at location: " <<
nh.getNamespace() << search_param_name);
nh.getParam(search_param_name, urdf_string);
}
else
{
ROS_INFO_STREAM_NAMED("generic_hw_interface", "Waiting for model URDF on the ROS param server at location: " <<
nh.getNamespace() << param_name);
nh.getParam(param_name, urdf_string);
}
usleep(100000);
}
if (!urdf_model_->initString(urdf_string))
ROS_ERROR_STREAM_NAMED("generic_hw_interface", "Unable to load URDF model");
else
ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Received URDF from param server");
}
bool Action::reachAction(geometry_msgs::Pose pose_target, const std::string surface_name)
{
bool success(false);
if (verbose_)
ROS_INFO_STREAM_NAMED("pick_place:","Planning to the pose " << pose_target);
if (!move_group_)
return false;
moveit::planning_interface::MoveGroup::Plan plan;
// Prevent collision with table
if (!surface_name.empty())
move_group_->setSupportSurfaceName(surface_name);
move_group_->setPoseTarget(pose_target, move_group_->getEndEffectorLink().c_str());
pub_obj_pose.publish(move_group_->getPoseTarget());
//move_group_->setMaxVelocityScalingFactor(0.6); //TODO back
//move_group_->setNumPlanningAttempts(50);
double tolerance_min = 0.01;
double tolerance_max = 0.5;
double tolerance = tolerance_min;
int attempts = 0;
//find a planning solution while increasing tolerance
while (!success && (attempts < 5))
{
move_group_->setGoalTolerance(tolerance);//0.05 //TODO to check
//move_group_->setGoalPositionTolerance(0.07);
//move_group_->setGoalOrientationTolerance(0.1);
success = move_group_->plan(plan);
if (!success)
{
tolerance_min = tolerance;
tolerance = (tolerance_max-tolerance_min) / 2.0;
if (verbose_)
ROS_INFO_STREAM_NAMED("pick_place:","Planning retry with the tolerance " << tolerance);
}
++attempts;
}
//find an approximate solution
if (!success)
{
move_group_->setApproximateJointValueTarget(pose_target, move_group_->getEndEffectorLink().c_str());
success = move_group_->plan(plan);
}
if (success)
success = move_group_->move();
if (verbose_ && success)
ROS_INFO_STREAM_NAMED("pick_place","Reaching success with tolerance " << tolerance << "\n\n");
return success;
}
void createCollisionObject(const geometry_msgs::Pose& block_pose, moveit_msgs::CollisionObject& block_object)
{
if( block_published_ )
{
return; // only publish the block once!
}
ROS_INFO_STREAM_NAMED("pick_place_moveit","Creating the collision object");
// ---------------------------------------------------------------------------------------------
// Create Solid Primitive
shape_msgs::SolidPrimitive block_shape;
// type of the shape
block_shape.type = shape_msgs::SolidPrimitive::BOX;
// dimensions of the shape
// block_shape.dimensions.resize(3);
block_shape.dimensions.push_back(BLOCK_SIZE); // x
block_shape.dimensions.push_back(BLOCK_SIZE); // y
block_shape.dimensions.push_back(BLOCK_SIZE); // z
// ---------------------------------------------------------------------------------------------
// Add the block to the collision environment
// a header, used for interpreting the poses
block_object.header.frame_id = base_link_;
block_object.header.stamp = ros::Time::now();
// the id of the object
static int block_id = 0;
block_object.id = "Block" + boost::lexical_cast<std::string>(block_id);
// the the collision geometries associated with the object;
// their poses are with respect to the specified header
// solid geometric primitives
//shape_msgs/SolidPrimitive[] primitives // TODO?
block_object.primitives.push_back(block_shape);
//geometry_msgs/Pose[] primitive_poses
block_object.primitive_poses.push_back( block_pose );
// meshes
//shape_msgs/Mesh[] meshes
//geometry_msgs/Pose[] mesh_poses
// bounding planes (equation is specified, but the plane can be oriented using an additional pose)
//shape_msgs/Plane[] planes
//geometry_msgs/Pose[] plane_poses
// Operation to be performed
block_object.operation = moveit_msgs::CollisionObject::ADD; // Puts the object into the environment or updates the object if already added
// Send the object
collision_obj_pub_.publish(block_object);
block_published_ = true;
ROS_INFO_STREAM_NAMED("pick_place_moveit","Collision object published for addition");
}
MovePlatformAction() :
as_(n_, CARLOS_MOVE_ACTION, false), //movePlatform action SERVER
ac_planner_("/plan_action", true), // Planner action CLIENT
ac_control_("/control_action", true) // Controller action CLIENT
{
n_.param("/move_platform_server/debug", debug_, false);
std::string name = ROSCONSOLE_DEFAULT_NAME; //ros.carlos_motion_action_server
name = name + ".debug";
logger_name_ = "debug";
//logger is ros.carlos_motion_action_server.debug
if (debug_)
{
// if we use ROSCONSOLE_DEFAULT_NAME we'll get a ton of debug messages from actionlib which is annoying!!!
// so for debug we'll use a named logger
if(ros::console::set_logger_level(name, ros::console::levels::Debug)) //name
ros::console::notifyLoggerLevelsChanged();
}
else // if not DEBUG we want INFO
{
if(ros::console::set_logger_level(name, ros::console::levels::Info)) //name
ros::console::notifyLoggerLevelsChanged();
}
ROS_DEBUG_NAMED(logger_name_, "Starting Move Platform Server");
as_.registerGoalCallback(boost::bind(&MovePlatformAction::moveGoalCB, this));
as_.registerPreemptCallback(boost::bind(&MovePlatformAction::movePreemptCB, this));
//start the move server
as_.start();
ROS_DEBUG_NAMED(logger_name_, "Move Platform Action Server Started");
// now wait for the other servers (planner + controller) to start
ROS_WARN_NAMED(logger_name_, "Waiting for planner server to start");
ac_planner_.waitForServer();
ROS_INFO_STREAM_NAMED(logger_name_, "Planner server started: " << ac_planner_.isServerConnected());
ROS_WARN_NAMED(logger_name_, "Waiting for controller server to start");
ac_control_.waitForServer();
ROS_INFO_STREAM_NAMED(logger_name_, "Controller server started: " << ac_control_.isServerConnected());
n_.param("/carlos/fsm_frequency", frequency_, DEFAULT_STATE_FREQ);
state_pub_timer_ = n_.createTimer(frequency_, &MovePlatformAction::state_pub_timerCB, this);
state_pub_ = n_.advertise<mission_ctrl_msgs::hardware_state>(CARLOS_BASE_STATE_MSG,1);
planning_ = false;
controlling_ = false;
//set_terminal_state_;
ctrl_state_sub = n_.subscribe<std_msgs::String>("/oea_controller/controller_state", 5, &MovePlatformAction::control_stateCB, this);
planner_state_sub = n_.subscribe<std_msgs::UInt8>("/oea_planner/state", 5, &MovePlatformAction::planner_stateCB, this);
}
void OpenNIListener::setupSubscribers(){
ParameterServer* ps = ParameterServer::instance();
int q = ps->get<int>("subscriber_queue_size");
ros::NodeHandle nh;
tflistener_ = new tf::TransformListener(nh);
std::string bagfile_name = ps->get<std::string>("bagfile_name");
if(bagfile_name.empty()){
std::string visua_tpc = ps->get<std::string>("topic_image_mono");
std::string depth_tpc = ps->get<std::string>("topic_image_depth");
std::string cinfo_tpc = ps->get<std::string>("camera_info_topic");
std::string cloud_tpc = ps->get<std::string>("topic_points");
std::string widev_tpc = ps->get<std::string>("wide_topic");
std::string widec_tpc = ps->get<std::string>("wide_cloud_topic");
//All information from Kinect
if(!visua_tpc.empty() && !depth_tpc.empty() && !cloud_tpc.empty())
{
visua_sub_ = new image_sub_type(nh, visua_tpc, q);
depth_sub_ = new image_sub_type (nh, depth_tpc, q);
cloud_sub_ = new pc_sub_type (nh, cloud_tpc, q);
kinect_sync_ = new message_filters::Synchronizer<KinectSyncPolicy>(KinectSyncPolicy(q), *visua_sub_, *depth_sub_, *cloud_sub_),
kinect_sync_->registerCallback(boost::bind(&OpenNIListener::kinectCallback, this, _1, _2, _3));
ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to " << visua_tpc << ", " << depth_tpc << " and " << cloud_tpc);
}
//No cloud, but visual image and depth
else if(!visua_tpc.empty() && !depth_tpc.empty() && !cinfo_tpc.empty() && cloud_tpc.empty())
{
visua_sub_ = new image_sub_type(nh, visua_tpc, q);
depth_sub_ = new image_sub_type(nh, depth_tpc, q);
cinfo_sub_ = new cinfo_sub_type(nh, cinfo_tpc, q);
no_cloud_sync_ = new message_filters::Synchronizer<NoCloudSyncPolicy>(NoCloudSyncPolicy(q), *visua_sub_, *depth_sub_, *cinfo_sub_);
no_cloud_sync_->registerCallback(boost::bind(&OpenNIListener::noCloudCallback, this, _1, _2, _3));
ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to " << visua_tpc << " and " << depth_tpc);
}
//All information from stereo
if(!widec_tpc.empty() && !widev_tpc.empty())
{
visua_sub_ = new image_sub_type(nh, widev_tpc, q);
cloud_sub_ = new pc_sub_type(nh, widec_tpc, q);
stereo_sync_ = new message_filters::Synchronizer<StereoSyncPolicy>(StereoSyncPolicy(q), *visua_sub_, *cloud_sub_);
stereo_sync_->registerCallback(boost::bind(&OpenNIListener::stereoCallback, this, _1, _2));
ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to " << widev_tpc << " and " << widec_tpc );
}
if(ps->get<bool>("use_robot_odom")){
odom_sub_= new odom_sub_type(nh, ps->get<std::string>("odometry_tpc"), 3);
ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to odometry on " << ps->get<std::string>("odometry_tpc"));
odom_sub_->registerCallback(boost::bind(&OpenNIListener::odomCallback,this,_1));
}
}
else {
ROS_WARN("RGBDSLAM loads a bagfile - therefore doesn't subscribe to topics.");
}
}
// Skip perception
void fake_goalCB()
{
ROS_INFO_STREAM_NAMED("pick_place_moveit","Received fake goal ----------------------------------------");
// Position
geometry_msgs::Pose start_block_pose;
geometry_msgs::Pose end_block_pose;
// Does not work
start_block_pose.position.x = 0.35;
start_block_pose.position.y = 0.2;
start_block_pose.position.z = 0.02;
// Works - close
start_block_pose.position.x = 0.2;
start_block_pose.position.y = 0.0;
start_block_pose.position.z = 0.02;
// 3rd try
start_block_pose.position.x = 0.35;
start_block_pose.position.y = 0.1;
start_block_pose.position.z = 0.02;
nh_.param<double>("/block_pick_place_server/block_x", start_block_pose.position.x, 0.2);
nh_.param<double>("/block_pick_place_server/block_y", start_block_pose.position.y, 0.0);
nh_.param<double>("/block_pick_place_server/block_z", start_block_pose.position.z, 0.02);
ROS_INFO_STREAM_NAMED("pick_place_moveit","start block is \n" << start_block_pose.position);
end_block_pose.position.x = 0.25;
end_block_pose.position.y = 0.15;
end_block_pose.position.z = 0.02;
// Orientation
double angle = M_PI / 1.5;
Eigen::Quaterniond quat(Eigen::AngleAxis<double>(double(angle), Eigen::Vector3d::UnitZ()));
start_block_pose.orientation.x = quat.x();
start_block_pose.orientation.y = quat.y();
start_block_pose.orientation.z = quat.z();
start_block_pose.orientation.w = quat.w();
angle = M_PI / 1.1;
quat = Eigen::Quaterniond(Eigen::AngleAxis<double>(double(angle), Eigen::Vector3d::UnitZ()));
end_block_pose.orientation.x = quat.x();
end_block_pose.orientation.y = quat.y();
end_block_pose.orientation.z = quat.z();
end_block_pose.orientation.w = quat.w();
// Fill goal
base_link_ = "base_link";
processGoal(start_block_pose, end_block_pose);
}
BlockPerceptionServer(const std::string name) :
nh_("~"),
action_server_(name, false),
action_name_(name)
{
// Publish a point cloud of filtered data that was not part of table
filtered_pub_ = nh_.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("block_output", 1);
// Publish a point cloud of data that was considered part of the plane
plane_pub_ = nh_.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("plane_output", 1);
// Publish interactive markers for blocks
block_pose_pub_ = nh_.advertise< geometry_msgs::PoseArray >("block_orientation", 1, true);
// Publish markers to highlight blocks
block_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("block_marker", 1);
// Setup OpenCV stuff
canny_threshold = 100;
hough_rho = 2; // Distance resolution of the accumulator in pixels.
hough_theta = 1; // Angle resolution of the accumulator in fraction of degress (so 1/theta degrees). to be converted to radians
hough_threshold = 14; // Accumulator threshold parameter. Only those lines are returned that get enough votes
hough_minLineLength = 13; //10; // Minimum line length. Line segments shorter than that are rejected.
hough_maxLineGap = 16; // Maximum allowed gap between points on the same line to link them.
// Initialize how often we process images
process_count_ = PROCESS_EVERY_NTH;
// TODO: move this, should be brought in from action goal. temporary!
base_link = "/base_link";
camera_link = "/camera_rgb_frame";
// camera_link = "/camera_rgb_optical_frame";
block_size = 0.04;
// table_height = 0.001;
table_height = 0.0;
// Subscribe to point cloud
point_cloud_sub_ = nh_.subscribe("/camera/depth_registered/points", 1, &BlockPerceptionServer::pointCloudCallback, this);
// Register the goal and feeback callbacks.
action_server_.registerGoalCallback(boost::bind(&BlockPerceptionServer::goalCB, this));
action_server_.registerPreemptCallback(boost::bind(&BlockPerceptionServer::preemptCB, this));
action_server_.start();
// Announce state
ROS_INFO_STREAM_NAMED("perception", "Server ready.");
ROS_INFO_STREAM_NAMED("perception", "Waiting for point clouds...");
}
void SplineFitting::calcSimpleDerivative()
{
ROS_INFO_STREAM_NAMED(name_, "Calculating simple derivatives");
joint_velocities_.resize(num_joints_);
bool verbose = false;
// For each joint
for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
{
// For each waypoint
for (std::size_t i = 1; i < joint_positions_[joint_id].size(); ++i)
{
double pos_diff = joint_positions_[joint_id][i] - joint_positions_[joint_id][i - 1];
double t_diff = timestamps_[i] - timestamps_[i - 1];
double vel = pos_diff / t_diff;
if (verbose)
std::cout << "joint_id: " << joint_id << " i: " << std::setfill('0') << std::setw(2) << i
<< " pos_diff: " << std::fixed << pos_diff << " \tt_diff: " << t_diff << " \tvel: " << vel
<< std::endl;
joint_velocities_[joint_id].push_back(vel);
}
joint_velocities_[joint_id].push_back(0.0); // TODO(davetcoleman): how to calculate final derivative?
}
}
void SplineFitting::getPPTrajectory(TOPP::Trajectory& trajectory)
{
ROS_INFO_STREAM_NAMED(name_, "Converting coefficients to new format");
std::list<TOPP::Chunk> chunks_list(timestamps_.size());
std::list<TOPP::Chunk>::iterator chunk_it = chunks_list.begin();
std::vector<TOPP::Polynomial> polynomials_vector(num_joints_);
std::vector<TOPP::dReal> coefficients_vector(4);
for (std::size_t chunk_id = 0; chunk_id < timestamps_.size() - 1; ++chunk_id) // TODO(davetcoleman): why is it minus one?
{
for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
{
for (std::size_t coeff_id = 0; coeff_id < 4; ++coeff_id)
{
coefficients_vector[coeff_id] = coefficients_[joint_id][chunk_id * 4 + coeff_id];
}
polynomials_vector[joint_id] = TOPP::Polynomial(coefficients_vector);
}
double duration = timestamps_[chunk_id + 1] - timestamps_[chunk_id];
//chunks_list[chunk_id] = TOPP::Chunk(duration, polynomials_vector);
(*chunk_it) = TOPP::Chunk(duration, polynomials_vector);
chunk_it++;
}
// Create final trajectory
trajectory.InitFromChunksList(chunks_list);
}
/**
* @brief Send a safety zone (volume), which is defined by two corners of a cube,
* to the FCU.
*
* @note ENU frame.
*/
void send_safety_set_allowed_area(Eigen::Vector3d p1, Eigen::Vector3d p2)
{
ROS_INFO_STREAM_NAMED("safetyarea", "SA: Set safty area: P1 " << p1 << " P2 " << p2);
p1 = ftf::transform_frame_enu_ned(p1);
p2 = ftf::transform_frame_enu_ned(p2);
mavlink::common::msg::SAFETY_SET_ALLOWED_AREA s;
m_uas->msg_set_target(s);
// TODO: use enum from lib
s.frame = utils::enum_value(mavlink::common::MAV_FRAME::LOCAL_NED);
// [[[cog:
// for p in range(1, 3):
// for v in ('x', 'y', 'z'):
// cog.outl("s.p%d%s = p%d.%s();" % (p, v, p, v))
// ]]]
s.p1x = p1.x();
s.p1y = p1.y();
s.p1z = p1.z();
s.p2x = p2.x();
s.p2y = p2.y();
s.p2z = p2.z();
// [[[end]]] (checksum: c996a362f338fcc6b714c8be583c3be0)
UAS_FCU(m_uas)->send_message_ignore_drop(s);
}
Chroma_processing::Chroma_processing()
: it_(nh_)
{
//Getting the parameters specified by the launch file
ros::NodeHandle local_nh("~");
local_nh.param("image_topic", image_topic, string("/camera/rgb/image_raw"));
local_nh.param("image_out_topic", image_out_topic, string("/chroma_proc/image"));
local_nh.param("image_out_dif_topic", image_out_dif_topic, string("/chroma_proc/image_dif"));
local_nh.param("project_path",path_, string(""));
local_nh.param("playback_topics", playback_topics, false);
local_nh.param("display", display, false);
if(playback_topics)
{
ROS_INFO_STREAM_NAMED("Chroma_processing","Subscribing at compressed topics \n");
image_sub = it_.subscribe(image_topic, 1,
&Chroma_processing::imageCb, this, image_transport::TransportHints("compressed"));
}
else
{
// Subscribe to input video feed
image_sub = it_.subscribe(image_topic, 1, &Chroma_processing::imageCb, this);
}
image_pub = it_.advertise(image_out_topic, 1);
image_pub_dif = it_.advertise(image_out_dif_topic, 1);
}
int main( int argc, char **argv )
{
ros::init( argc, argv, "example3" );
ros::NodeHandle n;
ros::Rate rate( 1 );
while( ros::ok() ) {
ROS_DEBUG_STREAM( "DEBUG message." );
ROS_INFO_STREAM ( "INFO message." );
ROS_WARN_STREAM ( "WARN message." );
ROS_ERROR_STREAM( "ERROR message." );
ROS_FATAL_STREAM( "FATAL message." );
ROS_INFO_STREAM_NAMED( "named_msg", "INFO named message." );
ROS_INFO_STREAM_THROTTLE( 2, "INFO throttle message." );
ros::spinOnce();
rate.sleep();
}
return EXIT_SUCCESS;
}
void initialize(UAS &uas_)
{
bool tf_listen;
uas = &uas_;
// main params
sp_nh.param("reverse_throttle", reverse_throttle, false);
// tf params
sp_nh.param("tf/listen", tf_listen, false);
sp_nh.param<std::string>("tf/frame_id", tf_frame_id, "local_origin");
sp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "attitude");
sp_nh.param("tf/rate_limit", tf_rate, 10.0);
if (tf_listen) {
ROS_INFO_STREAM_NAMED("attitude", "Listen to desired attitude transform " << tf_frame_id
<< " -> " << tf_child_frame_id);
tf2_start("AttitudeSpTF", &SetpointAttitudePlugin::transform_cb);
}
else {
twist_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointAttitudePlugin::twist_cb, this);
pose_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cb, this);
}
throttle_sub = sp_nh.subscribe("att_throttle", 10, &SetpointAttitudePlugin::throttle_cb, this);
}
/**
* Sent STATUSTEXT message to rosout
*
* @param[in] severity Levels defined in common.xml
*/
void process_statustext_normal(uint8_t severity, std::string &text) {
switch (severity) {
case MAV_SEVERITY_EMERGENCY:
case MAV_SEVERITY_ALERT:
case MAV_SEVERITY_CRITICAL:
case MAV_SEVERITY_ERROR:
ROS_ERROR_STREAM_NAMED("fcu", "FCU: " << text);
break;
case MAV_SEVERITY_WARNING:
case MAV_SEVERITY_NOTICE:
ROS_WARN_STREAM_NAMED("fcu", "FCU: " << text);
break;
case MAV_SEVERITY_INFO:
ROS_INFO_STREAM_NAMED("fcu", "FCU: " << text);
break;
case MAV_SEVERITY_DEBUG:
ROS_DEBUG_STREAM_NAMED("fcu", "FCU: " << text);
break;
default:
ROS_WARN_STREAM_NAMED("fcu", "FCU: UNK(" <<
int(severity) << "): " << text);
break;
};
}
void initialize(UAS &uas_,
ros::NodeHandle &nh,
diagnostic_updater::Updater &diag_updater)
{
bool pose_with_covariance;
bool listen_tf;
uas = &uas_;
sp_nh = ros::NodeHandle(nh, "position");
sp_nh.param("vision/pose_with_covariance", pose_with_covariance, false);
sp_nh.param("vision/listen_tf", listen_tf, false);
sp_nh.param<std::string>("vision/frame_id", frame_id, "local_origin");
sp_nh.param<std::string>("vision/child_frame_id", child_frame_id, "vision");
sp_nh.param("vision/tf_rate_limit", tf_rate, 50.0);
ROS_DEBUG_STREAM_NAMED("position", "Vision pose topic type: " <<
((pose_with_covariance)? "PoseWithCovarianceStamped" : "PoseStamped"));
if (listen_tf) {
ROS_INFO_STREAM_NAMED("position", "Listen to vision transform " << frame_id
<< " -> " << child_frame_id);
tf_start("VisionTF", &VisionPoseEstimatePlugin::send_vision_transform);
}
else if (pose_with_covariance)
vision_sub = sp_nh.subscribe("vision", 10, &VisionPoseEstimatePlugin::vision_cov_cb, this);
else
vision_sub = sp_nh.subscribe("vision", 10, &VisionPoseEstimatePlugin::vision_cb, this);
}
Depth_processing::Depth_processing()
: it_(nh_)
{
//Getting the parameters specified by the launch file
ros::NodeHandle local_nh("~");
local_nh.param("depth_topic" , depth_topic , string("/camera/depth/image_raw"));
local_nh.param("depth_out_image_topic" , depth_out_image_topic , string("/depth_proc/image"));
local_nh.param("project_path" ,path_ , string(""));
local_nh.param("playback_topics" , playback_topics ,false);
local_nh.param("display" , display , false);
local_nh.param("max_depth" , max_depth , DEPTH_MAX);
local_nh.param("min_depth" , min_depth , DEPTH_MIN);
if(playback_topics)
{
ROS_INFO_STREAM_NAMED("Depth_processing","Subscribing at compressed topics \n");
depth_sub = it_.subscribe(depth_topic, 10,
&Depth_processing::depthCb, this, image_transport::TransportHints("compressedDepth"));
}
else
depth_sub = it_.subscribe(depth_topic, 10, &Depth_processing::depthCb, this);
depth_pub = it_.advertise(depth_out_image_topic, 100);
}
int main(int argc, char *argv[])
{
int num_tests = 100;
ros::init(argc, argv, "grasp_generator_test");
// Allow the action server to recieve and send ros messages
ros::AsyncSpinner spinner(5);
spinner.start();
// Seed random
srand(ros::Time::now().toSec());
// Benchmark time
ros::Time start_time;
start_time = ros::Time::now();
// Run Tests
block_grasp_generator::GraspGeneratorTest tester(num_tests);
// Benchmark time
double duration = (ros::Time::now() - start_time).toNSec() * 1e-6;
ROS_INFO_STREAM_NAMED("","Total time: " << duration);
std::cout << duration << "\t" << num_tests << std::endl;
ros::Duration(1.0).sleep(); // let rviz markers finish publishing
return 0;
}
RigidTransformationPtr BodyTrajectory::getRTwithMaxTranslationDistance()
{
if (!this->_trans_params_estimated)
{
this->estimateTranslationParameters();
}
ROS_INFO_STREAM_NAMED("BodyTrajectory.getRTwithMaxTranslationDistance", "Maximum translation in frame " << this->_max_trans_frame);
return this->getRigidTransformation(this->_max_trans_frame);
}
RigidTransformationPtr BodyTrajectory::getRTwithMaxRotationAngle()
{
if (!this->_rot_params_estimated)
{
this->estimateRotationParameters();
}
ROS_INFO_STREAM_NAMED("BodyTrajectory.getRTwithMaxRotationAngle", "Maximum rotation in frame " << this->_max_rot_frame);
return this->getRigidTransformation(this->_max_rot_frame);
}
bool Action::executeAction()//execute
{
bool success(false);
if (verbose_)
ROS_INFO_STREAM_NAMED("pick_place:","Execution of the planned action");
if (!move_group_)
return false;
if (current_plan_)
success = move_group_->execute(*current_plan_);
if (verbose_ && success)
ROS_INFO_STREAM_NAMED("pick_place","Execute success! \n\n");
return success;
}
// Action server sends goals here
void goalCB()
{
ROS_INFO_STREAM_NAMED("pick_place_moveit","Received goal -----------------------------------------------");
pick_place_goal_ = action_server_.acceptNewGoal();
base_link_ = pick_place_goal_->frame;
processGoal(pick_place_goal_->pickup_pose, pick_place_goal_->place_pose);
}
void UrHardwareInterface::init() {
ROS_INFO_STREAM_NAMED("ur_hardware_interface",
"Reading rosparams from namespace: " << nh_.getNamespace());
// Get joint names
nh_.getParam("hardware_interface/joints", joint_names_);
if (joint_names_.size() == 0) {
ROS_FATAL_STREAM_NAMED("ur_hardware_interface",
"No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace());
exit(-1);
}
num_joints_ = joint_names_.size();
// Resize vectors
joint_position_.resize(num_joints_);
joint_velocity_.resize(num_joints_);
joint_effort_.resize(num_joints_);
joint_position_command_.resize(num_joints_);
joint_velocity_command_.resize(num_joints_);
// Initialize controller
for (std::size_t i = 0; i < num_joints_; ++i) {
ROS_DEBUG_STREAM_NAMED("ur_hardware_interface",
"Loading joint name: " << joint_names_[i]);
// Create joint state interface
joint_state_interface_.registerHandle(
hardware_interface::JointStateHandle(joint_names_[i],
&joint_position_[i], &joint_velocity_[i],
&joint_effort_[i]));
// Create position joint interface
position_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_position_command_[i]));
// Create velocity joint interface
velocity_joint_interface_.registerHandle(
hardware_interface::JointHandle(
joint_state_interface_.getHandle(joint_names_[i]),
&joint_velocity_command_[i]));
}
// Create force torque interface
force_torque_interface_.registerHandle(
hardware_interface::ForceTorqueSensorHandle("wrench", "",
robot_force_, robot_torque_));
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&position_joint_interface_); // From RobotHW base class.
registerInterface(&velocity_joint_interface_); // From RobotHW base class.
registerInterface(&force_torque_interface_); // From RobotHW base class.
velocity_interface_running_ = false;
position_interface_running_ = false;
}
// *Requires that the object already be created
void deleteCollisionObject(moveit_msgs::CollisionObject& block_object)
{
// Operation to be performed
block_object.operation = moveit_msgs::CollisionObject::REMOVE;
// Send the object
block_published_ = false;
collision_obj_pub_.publish(block_object);
ROS_INFO_STREAM_NAMED("pick_place_moveit","Collision object published for removal");
}
void SplineFitting::writeCoefficientsToFile(const std::string& file_path)
{
ROS_INFO_STREAM_NAMED(name_, "Writing coefficients to file");
// Open file
std::ofstream output_handle;
output_handle.open(file_path.c_str());
for (std::size_t i = 0; i < timestamps_.size() - 1; ++i)
{
std::cout << "timestamp " << timestamps_[i] << std::endl;
// Timestamp
if (i == 0)
{
output_handle << timestamps_[i] << std::endl;
}
else
{
output_handle << timestamps_[i] - timestamps_[i-1] << std::endl;
}
// Num joints
output_handle << num_joints_ << std::endl;
// Coefficients
output_handle.precision(10);
for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
{
for (std::size_t c = 0; c < 4; ++c)
{
//std::cout << "joint_id " << joint_id << " then " << i << " then " << c << " = " << (i * 4 + (3 - c)) << std::endl;
output_handle << coefficients_[joint_id][i * 4 + (3 - c)];
if (c < 3)
output_handle << ",";
}
output_handle << std::endl;
}
}
// Save
output_handle.close();
ROS_INFO_STREAM_NAMED(name_, "Saved coefficients to file " << file_path);
}
void goalCB()
{
ROS_INFO_STREAM_NAMED("perception","Current scene requested");
// Accept the new goal and save data
goal_ = action_server_.acceptNewGoal();
block_size = goal_->block_size;
table_height = goal_->table_height;
base_link = goal_->frame;
}
// Constructor
GraspGeneratorTest(int num_tests) :
nh_("~")
{
// ---------------------------------------------------------------------------------------------
// Load the Robot Viz Tools for publishing to Rviz
rviz_tools_.reset(new block_grasp_generator::RobotVizTools(RVIZ_MARKER_TOPIC, EE_GROUP, PLANNING_GROUP_NAME, BASE_LINK));
rviz_tools_->setLifetime(40.0);
rviz_tools_->setMuted(false);
// ---------------------------------------------------------------------------------------------
// Load grasp generator
loadRobotGraspData(); // Load robot specific data
block_grasp_generator_.reset( new block_grasp_generator::BlockGraspGenerator(rviz_tools_) );
// ---------------------------------------------------------------------------------------------
// Load grasp filter
bool rviz_verbose = true;
grasp_filter_.reset(new block_grasp_generator::GraspFilter(BASE_LINK, rviz_verbose, rviz_tools_, PLANNING_GROUP_NAME) );
// ---------------------------------------------------------------------------------------------
// Generate grasps for a bunch of random blocks
geometry_msgs::Pose block_pose;
std::vector<manipulation_msgs::Grasp> possible_grasps;
// Loop
for (int i = 0; i < num_tests; ++i)
{
ROS_INFO_STREAM_NAMED("test","Adding random block " << i+1 << " of " << num_tests);
generateRandomBlock(block_pose);
//getTestBlock(block_pose);
rviz_tools_->publishBlock(block_pose, BLOCK_SIZE, false);
possible_grasps.clear();
// Generate set of grasps for one block
rviz_tools_->setMuted(true); // we don't want to see unfiltered grasps
block_grasp_generator_->generateGrasps( block_pose, grasp_data_, possible_grasps);
rviz_tools_->setMuted(false);
// Filter the grasp for only the ones that are reachable
grasp_filter_->filterGrasps(possible_grasps);
// Visualize them
block_grasp_generator_->visualizeGrasps(possible_grasps, block_pose);
// Make sure ros is still going
if(!ros::ok)
break;
}
}
