void ApproximateValueManager::Initialize( BroadcastMultiReceiver::Ptr inputs,
ros::NodeHandle& ph )
{
WriteLock lock( _mutex );
_receiver = inputs;
unsigned int inputDim = _receiver->GetDim();
ros::NodeHandle vh( ph.resolveName( "approximator" ) );
_value.Initialize( inputDim, vh );
_getParamServer = ph.advertiseService( "get_params", &ApproximateValueManager::GetParamsCallback, this );
_setParamServer = ph.advertiseService( "set_params", &ApproximateValueManager::SetParamsCallback, this );
std::string valueName;
GetParamRequired( ph, "value_name", valueName );
register_lookup_target( ph, valueName );
ValueInfo info;
info.inputDim = _receiver->GetDim();
info.paramQueryService = ph.resolveName( "get_params" );
info.paramSetService = ph.resolveName( "set_params" );
GetParamRequired( vh, "", info.approximatorInfo );
if( !_infoManager.WriteMemberInfo( valueName, info, true, ros::Duration( 10.0 ) ) )
{
throw std::runtime_error( "Could not write value info!" );
}
}
TransformPointcloudNode (ros::NodeHandle &n) : nh_(n)
{
// Maximum number of outgoing messages to be queued for delivery to subscribers = 1
nh_.param("input_cloud_topic", input_cloud_topic_, std::string("cloud_pcd"));
output_cloud_topic_ = input_cloud_topic_ + "_transformed";
nh_.param("to_frame", to_frame_, std::string("base_link"));
nh_.param("point_cloud_name", point_cloud_name_, std::string("pc"));
nh_.param("save_point_cloud", save_point_cloud_, false);
sub_ = nh_.subscribe (input_cloud_topic_, 1, &TransformPointcloudNode::cloud_cb, this);
ROS_INFO ("[TransformPointcloudNode:] Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ());
pub_ = nh_.advertise<sensor_msgs::PointCloud2>(output_cloud_topic_, 1);
ROS_INFO ("[TransformPointcloudNode:] Will be publishing data on topic %s.", nh_.resolveName (output_cloud_topic_).c_str ());
}
// constructor
online_IPM(ros::NodeHandle nh, int ow, int oh, double f_u, double f_v, double c_u, double c_v, double deg, double cam_h, std::string camera_name)
: nh_(nh), priv_nh_("~"),ipMapper(ow,oh,f_u,f_v,c_u,c_v,deg,cam_h),it(nh_)
{
read_images_= nh_.subscribe(nh_.resolveName(camera_name), 1,&online_IPM::publish_remapper,this);
pub_mapped_images_= it.advertise("/camera/ground_image_ipmapped", 1);
//ipMapper.initialize(200,PROJECTED_IMAGE_HEIGTH, fu, fv, cx, cy, pitch);
}
void ApproximateValueManager::Initialize( ros::NodeHandle& ph )
{
ros::NodeHandle ih( ph.resolveName( "input_streams" ) );
BroadcastMultiReceiver::Ptr rx = std::make_shared<BroadcastMultiReceiver>();
rx->Initialize( ih );
Initialize( rx, ph );
}
void
setupPubs()
{
//look up remapping
std::string topic = nh_.resolveName(topic_, true);
pub_ = nh_.advertise<MessageT>(topic, queue_size_, latched_);
ROS_INFO_STREAM("publishing to topic:" << topic);
}
// constructor
PoseTracker(ros::NodeHandle nh, int argc,char** argv) : nh_(nh), priv_nh_("~"), filter_()
{
priv_nh_.param<std::string>("reference_frame", reference_frame_, "");
priv_nh_.param<std::string>("object_frame", object_frame_, "");
// get the led location in local frame from ros param server
nh_.getParam(object_frame_ + std::string("_leds"), leds_);
parseParameters(leds_);
transfParameters_.resize(7);
// initialize subscriber
sub_phase_space_markers_ = nh_.subscribe(nh_.resolveName("/phase_space_markers"), 10, &PoseTracker::estimatePose,this);
string tansform_name =reference_frame_+"_to_"+object_frame_;
pub_transform_= nh.advertise<geometry_msgs::TransformStamped>(nh.resolveName(tansform_name.c_str()), 10);
}
calc_flow_node () : nh_(), it_(nh_), flow(0, 0, CV_8UC1), prevImg(0, 0, CV_8UC1) {
//flow = new cv::Mat(0, 0, CV_8UC1);
namespace_ = nh_.resolveName("camera");
camera_sub = it_.subscribeCamera(namespace_ + "/image", 10, &calc_flow_node::cameraCB, this);
result_pub_ = nh_.advertise<sensor_msgs::Image> ("flow_image", 1);
//result_pub_ = nh_.advertise<sensor_msgs::PointCloud2> ("flow_vector", 1);
}
FiducialPoseEstimator( ros::NodeHandle& nh, ros::NodeHandle& ph )
: _fiducialManager( _lookupInterface ),
_extrinsicsInterface( nh, ph )
{
GetParam<std::string>( ph, "reference_frame", _refFrame, "" );
GetParam<std::string>( ph, "body_frame", _robotFrame, "base_link");
bool combineDetect;
GetParam( ph, "combine_detections", combineDetect, true);
unsigned int inBuffSize, outBuffSize;
GetParam( ph, "input_buffer_size", inBuffSize, (unsigned int) 20 );
GetParam( ph, "output_buffer_size", outBuffSize, (unsigned int) 20 );
_enableVis = ph.hasParam( "visualization" );
if( _enableVis )
{
ros::NodeHandle ch( ph.resolveName( "visualization/camera" ) );
ros::NodeHandle fh( ph.resolveName( "visualization/fiducial" ) );
_camVis.ReadParams( ch );
_fidVis.ReadParams( fh );
std::string refFrame;
GetParamRequired( ph, "visualization/reference_frame", refFrame );
_camVis.SetFrameID( refFrame );
_fidVis.SetFrameID( refFrame );
_visPub = nh.advertise<MarkerMsg>( "markers", 10 );
}
if( combineDetect )
{
_detSub = nh.subscribe( "detections",
inBuffSize,
&FiducialPoseEstimator::DetectionsCallbackCombined,
this );
}
else
{
_detSub = nh.subscribe( "detections",
inBuffSize,
&FiducialPoseEstimator::DetectionsCallbackIndependent,
this );
}
_posePub = ph.advertise<geometry_msgs::TransformStamped>( "relative_pose",
outBuffSize );
}
/////////////////////////////////////////////////////////////////
// Constructor
PedestrianDetectorHOG(ros::NodeHandle nh):
nh_(nh),
local_nh_("~"),
it_(nh_),
stereo_sync_(4),
// cam_model_(NULL),
counter(0)
{
hog_.setSVMDetector(cv::HOGDescriptor::getDefaultPeopleDetector());
// Get parameters from the server
local_nh_.param("hit_threshold",hit_threshold_,0.0);
local_nh_.param("group_threshold",group_threshold_,2);
local_nh_.param("use_depth", use_depth_, true);
local_nh_.param("use_height",use_height_,true);
local_nh_.param("max_height_m",max_height_m_,2.2);
local_nh_.param("ground_frame",ground_frame_,std::string("base_link"));
local_nh_.param("do_display", do_display_, true);
// Advertise a 3d position measurement for each head.
people_pub_ = nh_.advertise<people_msgs::PositionMeasurement>("people_tracker_measurements",1);
// Subscribe to tf & the images
if (use_depth_) {
tf_.setExtrapolationLimit(ros::Duration().fromSec(0.01));
std::string left_topic = nh_.resolveName("stereo") + "/left/image";
std::string disp_topic = nh_.resolveName("stereo") + "/disparity";
std::string left_info_topic = nh_.resolveName("stereo") + "/left/camera_info";
std::string right_info_topic = nh_.resolveName("stereo") + "/right/camera_info";
left_sub_.subscribe(it_, left_topic, 10);
disp_sub_.subscribe(it_, disp_topic, 10);
left_info_sub_.subscribe(nh_, left_info_topic, 10);
right_info_sub_.subscribe(nh_, right_info_topic, 10);
stereo_sync_.connectInput(left_sub_, left_info_sub_, disp_sub_, right_info_sub_);
stereo_sync_.registerCallback(boost::bind(&PedestrianDetectorHOG::imageCBAll, this, _1, _2, _3, _4));
}
else {
std::string topic = nh_.resolveName("image");
image_sub_ = it_.subscribe("image",10,&PedestrianDetectorHOG::imageCB,this);
}
}
CloudPublisher()
: tf_frame("/base_link"),
private_nh("~")
{
cloud_topic = "cloud";
pub.advertise(nh, cloud_topic.c_str(), 1);
private_nh.param("frame_id", tf_frame, std::string("/base_link"));
ROS_INFO_STREAM("Publishing data on topic \"" << nh.resolveName(cloud_topic) << "\" with frame_id \"" << tf_frame << "\"");
}
SegmentDifferencesNode (ros::NodeHandle &n) : nh_(n)
{
// Maximum number of outgoing messages to be queued for delivery to subscribers = 1
nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/cloud_pcd"));
nh_.param("output_cloud_topic", output_cloud_topic_, std::string("difference"));
nh_.param("output_filtered_cloud_topic", output_filtered_cloud_topic_, std::string("difference_filtered"));
nh_.param("distance_threshold", distance_threshold_, 0.0005);
ROS_INFO ("Distance threshold set to %lf.", distance_threshold_);
pub_diff_.advertise (nh_, output_cloud_topic_.c_str (), 1);
ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_cloud_topic_).c_str ());
pub_filtered_.advertise (nh_, output_filtered_cloud_topic_.c_str (), 1);
ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_filtered_cloud_topic_).c_str ());
sub_.subscribe (nh_, input_cloud_topic_, 1, boost::bind (&SegmentDifferencesNode::cloud_cb, this, _1));
ROS_INFO ("Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ());
//set PCL classes
seg_.setDistanceThreshold (distance_threshold_);
rate_ = 1;
counter_ = 0;
}
StereoSynchronizer () : nh_(), it_(nh_), sync_(15) {
std::string left_raw = nh_.resolveName("left_raw");
std::string right_raw = nh_.resolveName("right_raw");
image_sub_l_.subscribe(it_, left_raw + "/image_raw", 4);
info_sub_l_ .subscribe(nh_, left_raw + "/camera_info", 4);
image_sub_r_.subscribe(it_, right_raw + "/image_raw", 4);
info_sub_r_ .subscribe(nh_, right_raw + "/camera_info", 4);
left_ns_ = nh_.resolveName("left");
right_ns_ = nh_.resolveName("right");
ros::NodeHandle cam_l_nh(left_ns_);
ros::NodeHandle cam_r_nh(right_ns_);
it_l_ = new image_transport::ImageTransport(cam_l_nh);
it_r_ = new image_transport::ImageTransport(cam_r_nh);
pub_left_ = it_l_->advertiseCamera("image_raw", 1);
pub_right_ = it_r_->advertiseCamera("image_raw", 1);
sync_.connectInput(image_sub_l_, info_sub_l_, image_sub_r_, info_sub_r_);
sync_.registerCallback(boost::bind(&StereoSynchronizer::imageCB, this, _1, _2, _3, _4));
}
/*! Also attempts to connect to database */
Right_arm_planner(ros::NodeHandle nh) : nh_(nh), priv_nh_("~")
{
right_arm_planning_srv_ = nh_.advertiseService(nh_.resolveName("right_arm_planning_srv"), &Right_arm_planner::serviceCallback, this);
right_arm_planning_controller_pub_ = nh_.advertise<control_msgs::FollowJointTrajectoryActionGoal>("/right_arm/joint_trajectory_controller/follow_joint_trajectory/goal", 1, true);
//right_arm_planning_controller_pub_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("/right_arm/joint_trajectory_controller/follow_joint_trajectory/feedback", 1, true);
right_arm_planning_pub_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
}
HoughLines(ros::NodeHandle nh = ros::NodeHandle()) : it_(nh), nh_(nh), subscriber_count_(0)
{
image_transport::SubscriberStatusCallback connect_cb = boost::bind(&HoughLines::connectCb, this, _1);
image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&HoughLines::disconnectCb, this, _1);
img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "hough")).advertise("image", 1, connect_cb, disconnect_cb);
out_pub_ = nh.advertise<jsk_perception::LineArray>(nh_.resolveName("lines"), 1);
dynamic_reconfigure::Server<jsk_perception::HoughLinesConfig>::CallbackType f =
boost::bind(&HoughLines::reconfigureCallback, this, _1, _2);
srv.setCallback(f);
}
void DiscretePolicyManager::Initialize( DiscretePolicyInterface::Ptr& interface,
ros::NodeHandle& nh,
ros::NodeHandle& ph )
{
_interface = interface;
_actionPub = ph.advertise<DiscreteAction>( "actions", 0 );
_paramSub = nh.subscribe( "param_updates",
1,
&DiscretePolicyManager::ParamCallback,
this );
ros::NodeHandle subh( ph.resolveName("input_streams") );
_inputStreams.Initialize( subh );
unsigned int inputDim = _inputStreams.GetDim();
unsigned int outputDim = _interface->GetOutputSizes().array().prod();
unsigned int numHiddenLayers, layerWidth;
GetParamRequired( ph, "network/num_hidden_layers", numHiddenLayers );
GetParamRequired( ph, "network/layer_width", layerWidth );
_network = std::make_shared<NetworkType>( inputDim,
outputDim,
numHiddenLayers,
layerWidth );
_network->SetInputSource( &_networkInput );
_networkParameters = _network->CreateParameters();
VectorType w = _networkParameters->GetParamsVec();
percepto::randomize_vector( w, -0.1, 0.1 );
_networkParameters->SetParamsVec( w );
if( HasParam( ph, "seed" ) )
{
int seed;
GetParam( ph, "seed", seed );
_engine.seed( seed );
}
else
{
boost::random::random_device rng;
_engine.seed( rng() );
}
double updateRate;
GetParamRequired( ph, "update_rate", updateRate );
_timer = ph.createTimer( ros::Duration( 1.0/updateRate ),
&DiscretePolicyManager::UpdateCallback,
this );
}
bool spin ()
{
int nr_points = cloud.width * cloud.height;
std::string fields_list = pcl::getFieldsList(cloud);
ros::Rate r(ros::Duration(1,0)); //1s tact
while(nh.ok ())
{
ROS_DEBUG_STREAM_ONCE("Publishing data with " << nr_points
<< " points " << fields_list
<< " on topic \"" << nh.resolveName(cloud_topic)
<< "\" in frame \"" << cloud.header.frame_id << "\"");
cloud.header.stamp = ros::Time::now();
pub.publish(cloud);
r.sleep();
}
return (true);
}
ExtractImages(const ros::NodeHandle& nh, const std::string& transport)
: filename_format_(""), count_(0), _time(ros::Time::now().toSec())
{
std::string topic = nh.resolveName("image");
ros::NodeHandle local_nh("~");
std::string format_string;
local_nh.param("filename_format", format_string, std::string("frame%05i.png"));
filename_format_.parse(format_string);
local_nh.param("sec_per_frame", sec_per_frame_, 0.1);
image_transport::ImageTransport it(nh);
sub_ = it.subscribe(topic, 1, &ExtractImages::image_cb, this, transport);
#if defined(_VIDEO)
video_writer = 0;
#endif
ROS_INFO("Initialized sec per frame to %f", sec_per_frame_);
}
CameraSubscriber::CameraSubscriber(ImageTransport& image_it, ros::NodeHandle& info_nh,
const std::string& base_topic, uint32_t queue_size,
const Callback& callback, const ros::VoidPtr& tracked_object,
const TransportHints& transport_hints)
: impl_(new Impl(queue_size))
{
// Must explicitly remap the image topic since we then do some string manipulation on it
// to figure out the sibling camera_info topic.
std::string image_topic = info_nh.resolveName(base_topic);
std::string info_topic = getCameraInfoTopic(image_topic);
impl_->image_sub_.subscribe(image_it, image_topic, queue_size, transport_hints);
impl_->info_sub_ .subscribe(info_nh, info_topic, queue_size, transport_hints.getRosHints());
impl_->sync_.connectInput(impl_->image_sub_, impl_->info_sub_);
// need for Boost.Bind here is kind of broken
impl_->sync_.registerCallback(boost::bind(callback, _1, _2));
// Complain every 10s if it appears that the image and info topics are not synchronized
impl_->image_sub_.registerCallback(boost::bind(increment, &impl_->image_received_));
impl_->info_sub_.registerCallback(boost::bind(increment, &impl_->info_received_));
impl_->sync_.registerCallback(boost::bind(increment, &impl_->both_received_));
impl_->check_synced_timer_ = info_nh.createWallTimer(ros::WallDuration(10.0),
boost::bind(&Impl::checkImagesSynchronized, impl_));
}
void loop(){
cloud_topic_ = "input";
sub_ = nh_.subscribe ("input", 1, &VoxelizeAndDetect::vad_cb, this);
ROS_INFO ("Listening for incoming data on topic %s", nh_.resolveName (cloud_topic_).c_str ());
}
PointCloudConverter () : nh_ ("~"), queue_size_ (100),
points_in_ ("/points_in"), points2_in_ ("/points2_in"),
points_out_ ("/points_out"), points2_out_ ("/points2_out")
{
// Subscribe to the cloud topic using both the old message format and the new
sub_points_ = nh_.subscribe (points_in_, queue_size_, &PointCloudConverter::cloud_cb_points, this);
sub_points2_ = nh_.subscribe (points2_in_, queue_size_, &PointCloudConverter::cloud_cb_points2, this);
pub_points_ = nh_.advertise<sensor_msgs::PointCloud> (points_out_, queue_size_);
pub_points2_ = nh_.advertise<sensor_msgs::PointCloud2> (points2_out_, queue_size_);
ROS_INFO ("PointCloudConverter initialized to transform from PointCloud (%s) to PointCloud2 (%s).", nh_.resolveName (points_in_).c_str (), nh_.resolveName (points2_out_).c_str ());
ROS_INFO ("PointCloudConverter initialized to transform from PointCloud2 (%s) to PointCloud (%s).", nh_.resolveName (points2_in_).c_str (), nh_.resolveName (points_out_).c_str ());
}
dataPublisher(ros::NodeHandle nh) : nh_(nh), priv_nh_("~"),sub_markers_readings(nh,nh.resolveName("/all_markers"),1),sub_camera_readings(nh,"/camera/depth_registered/points",1),sub_starlink_readings(nh,"/world_star",1),sub_wristlink_readings(nh,"/world_wrist",1),sync(MySyncPolicy(10),sub_markers_readings,sub_camera_readings,sub_starlink_readings,sub_wristlink_readings)
{
sync.registerCallback( boost::bind( &dataPublisher::callback, this, _1, _2 ,_3,_4) );
data_info = nh_.advertise<AllData>(nh_.resolveName("/data_info"), 10);
}
void
cloud_cb_points2 (const sensor_msgs::PointCloud2ConstPtr &msg)
{
if (pub_points_.getNumSubscribers () <= 0)
{
//ROS_DEBUG ("[point_cloud_converter] Got a PointCloud2 with %d points on %s, but no subscribers.", msg->width * msg->height, nh_.resolveName (points2_in_).c_str ());
return;
}
ROS_DEBUG ("[point_cloud_converter] Got a PointCloud2 with %d points (%s) on %s.", msg->width * msg->height, getFieldsList (*msg).c_str (), nh_.resolveName (points2_in_).c_str ());
sensor_msgs::PointCloud output;
// Convert to the new PointCloud format
if (!sensor_msgs::convertPointCloud2ToPointCloud (*msg, output))
{
ROS_ERROR ("[point_cloud_converter] Conversion from sensor_msgs::PointCloud2 to sensor_msgs::PointCloud failed!");
return;
}
ROS_DEBUG ("[point_cloud_converter] Publishing a PointCloud with %d points on %s.", (int)output.points.size (), nh_.resolveName (points_out_).c_str ());
pub_points_.publish (output);
}
static std::string name_recorder(ros::NodeHandle &node, const std::string &name)
{
return (name == "" ? name::camcorder() : node.resolveName(name));
}
bool Right_arm_planner::serviceCallback(grasp_estimator::ArmPlanning::Request &request, grasp_estimator::ArmPlanning::Response &response)
{
grasp_estimator::ArmPlanning service_request;
service_request.request.goal=request.goal;
// Setup MoveIt enviroment
moveit::planning_interface::MoveGroup group("right_hand_arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit::planning_interface::MoveGroup::Plan simple_plan;
moveit_msgs::DisplayTrajectory display_trajectory;
group.allowReplanning ("true");
group.setPlannerId("PRMstarkConfigDefault");
group.setPlanningTime((double)1);
group.setGoalOrientationTolerance((double)0.005);
group.setGoalPositionTolerance((double)0.005);
//moveit_msgs::RobotTrajectory trajectory;
geometry_msgs::Pose start_pose;
geometry_msgs::Pose target_pose;
// Create useful variables
geometry_msgs::PoseStamped des_position;
std::vector<geometry_msgs::Pose> waypoints;
// Trajectory controller
trajectory_msgs::JointTrajectory trajectory;
control_msgs::FollowJointTrajectoryActionGoal trajectory_controller_msgs;
ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());
if(service_request.request.goal.decision==(int)1)
{
// checking and asking home trasform
double pi(3.1415);
double threshold_distance((double)0.02);
ROS_INFO("Listening for world_link_2_wrist_stamped");
tf::StampedTransform world_link_2_wrist_stamped;
ros::Time now0 = ros::Time::now();
listener_.waitForTransform("vito_anchor","right_hand_palm_dummy_link", now0, ros::Duration(4));
listener_.lookupTransform("vito_anchor","right_hand_palm_dummy_link", now0, world_link_2_wrist_stamped);
tf::Transform world_link_2_wrist;
world_link_2_wrist.setOrigin(world_link_2_wrist_stamped.getOrigin());
world_link_2_wrist.setBasis(world_link_2_wrist_stamped.getBasis());
tf::Vector3 position_wrist_home = world_link_2_wrist.getOrigin();
tf::Quaternion quaternion_wrist_home = world_link_2_wrist.getRotation();
tf::Transform wrist_pose;
wrist_pose.setOrigin(position_wrist_home);
wrist_pose.setRotation(quaternion_wrist_home);
tf::Matrix3x3 rotation_wrist_home;
rotation_wrist_home.setRotation(quaternion_wrist_home);
double roll_wrist, pitch_wrist, yaw_wrist;
rotation_wrist_home.getRPY(roll_wrist, pitch_wrist, yaw_wrist);
ROS_INFO("RPY_wrist = (%f, %f, %f)", roll_wrist, pitch_wrist, yaw_wrist);
ROS_INFO ("Right_arm_planner_srv:Success in receving wrist transformation with 7 elements (Orient(x,y,z,w) ,Pos(x,y,z)): %f, %f ,%f, %f, %f, %f, %f",quaternion_wrist_home[0],quaternion_wrist_home[1],quaternion_wrist_home[2],quaternion_wrist_home[3],position_wrist_home[0],position_wrist_home[1],position_wrist_home[2]);
// Setup moveit start position
start_pose.orientation.x = quaternion_wrist_home[0];
start_pose.orientation.y = quaternion_wrist_home[1];
start_pose.orientation.z = quaternion_wrist_home[2];
start_pose.orientation.w = quaternion_wrist_home[3];
start_pose.position.x = position_wrist_home[0];
start_pose.position.y = position_wrist_home[1];
start_pose.position.z = position_wrist_home[2];
// Receving desiderate position
des_position=request.goal.des_pose;
// check if the desidered pose is empty
if( des_position.pose.orientation.x==0 && des_position.pose.orientation.y==0 && des_position.pose.orientation.z==0 && des_position.pose.orientation.w==0 && des_position.pose.position.x==0 && des_position.pose.position.y==0 && des_position.pose.position.z==0)
{
ROS_ERROR("Desidered Position is empty");
response.result = response.NO_DES_POS_RECEIVED;
}
ROS_INFO ("Right_arm_planner_srv:Success in receving desidered wrist transformation with 7 elements (Orient(x,y,z,w) ,Pos(x,y,z)): %f, %f ,%f, %f, %f, %f, %f",des_position.pose.orientation.x,des_position.pose.orientation.y,des_position.pose.orientation.z,des_position.pose.orientation.w,des_position.pose.position.x,des_position.pose.position.y,des_position.pose.position.z);
tf::Transform des_wrist_pose(tf::Quaternion(des_position.pose.orientation.x, des_position.pose.orientation.y, des_position.pose.orientation.z, des_position.pose.orientation.w), tf::Vector3( des_position.pose.position.x, des_position.pose.position.y, des_position.pose.position.z));
// Get desidered wrist pose
//tf::Transform des_wrist_pose;
//tf::poseMsgToTF(des_position,des_wrist_pose);
tf::Matrix3x3 des_rotation_wrist = des_wrist_pose.getBasis();
tf::Vector3 des_position_wrist = des_wrist_pose.getOrigin();
tf::Quaternion des_orientation_wrist;
des_rotation_wrist.getRotation(des_orientation_wrist);
// check if the resulting orientation is correct
if (des_rotation_wrist.determinant() < 0.5 )
{
ROS_ERROR("Desidered Position is bad Posed");
response.result = response.DES_POS_BAD_POSE;
}
ROS_INFO("Desidered Position Received");
target_pose.position.x = des_position.pose.position.x;
target_pose.position.y = des_position.pose.position.y;
target_pose.position.z = des_position.pose.position.z;
target_pose.orientation.x = des_position.pose.orientation.x;
target_pose.orientation.y = des_position.pose.orientation.y;
target_pose.orientation.z = des_position.pose.orientation.z;
target_pose.orientation.w = des_position.pose.orientation.w;
//target_pose.orientation.w = 1;
//group.(target_pose);
ROS_INFO("Starting wrist planning for right arm");
ROS_INFO("Setting Pose Target");
group.setPoseTarget(target_pose);
ROS_INFO("Planning position Target");
group.setPlanningTime((double)5);
group.setGoalOrientationTolerance((double)0.005);
group.setGoalPositionTolerance((double)0.005);
bool success = group.plan(simple_plan);
ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
ROS_INFO("Visualizing simple plan ");
display_trajectory.trajectory_start = simple_plan.start_state_;
display_trajectory.trajectory.push_back(simple_plan.trajectory_);
right_arm_planning_pub_.publish(display_trajectory);
// Sleep to give Rviz time to visualize the plan.
sleep(3.0);
if (success==(bool)1)
{
group.move();
ROS_INFO("Robot Move DONE");
response.result=response.SUCCESS;
response.error = (float)0;
}
else
{
response.result=response.OTHER_ERROR;
response.error = (float)0;
}
}
if(service_request.request.goal.decision==(int)3)
{
ROS_INFO("Starting wrist planning for right arm");
std::vector<double> group_variable_values;
//group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);
ros::Time start_time = ros::Time::now();
std::string topic = nh_.resolveName("/right_arm/joint_states");
ROS_INFO(" Waiting for right arm joint state on topic %s", topic.c_str());
sensor_msgs::JointState::ConstPtr CurrentState_ConstPtr =
ros::topic::waitForMessage<sensor_msgs::JointState>(topic, nh_, ros::Duration(3.0));
if(!CurrentState_ConstPtr) ROS_ERROR("empty joint states!");
else ROS_INFO(" Received state is not empty for right arm joint state on topic %s", topic.c_str());
ROS_INFO(" Received state for right arm joint state on topic %s", topic.c_str());
// ROS_INFO("Joint position %f",float(CurrentState_ConstPtr->position[0]));
// ROS_INFO("Joint position %f",float(CurrentState_ConstPtr->position[1]));
// ROS_INFO("Joint position %f",float(CurrentState_ConstPtr->position[2]));
// ROS_INFO("Joint position %f",float(CurrentState_ConstPtr->position[3]));
// ROS_INFO("Joint position %f",float(CurrentState_ConstPtr->position[4]));
// ROS_INFO("Joint position %f",float(CurrentState_ConstPtr->position[5]));
// ROS_INFO("Joint position %f",float(CurrentState_ConstPtr->position[6]));
ROS_INFO("Start position received");
float des_joint_position[7];
int joint_change_position[7];
group_variable_values=CurrentState_ConstPtr->position;
int count =0;
for( int i = 0; i < 7; ++i) {
des_joint_position[i]=request.goal.joint_position[i];
joint_change_position[i]=request.goal.joint_change_position[i];
}
// int count=0;
for( int i = 0; i < 7; ++i)
{ //ROS_INFO("Cycle %f",float(i));
if (joint_change_position[i] ==(int)1)
{
group_variable_values[i] = des_joint_position[i];
count= count=+1;
}
}
if (count==7)
{
ROS_ERROR("joint change position is empty");
response.result = response.DES_CHANGE_JOINT_EMPTY;
}
ROS_INFO("Setting joint value target done");
group.setJointValueTarget(group_variable_values);
//sleep(5.0);
sleep(3.0);
group.move();// questo move group blocca il programma
response.result=response.SUCCESS;
response.error = (float)0;
//group.move();
ROS_INFO("Planning Done");
ROS_INFO("Visualizing simple plan (again)");
display_trajectory.trajectory_start = simple_plan.start_state_;
display_trajectory.trajectory.push_back(simple_plan.trajectory_);
right_arm_planning_pub_.publish(display_trajectory);
//ROS_INFO("First trajectory Point %f",trajectory_controller_msgs.goal.trajectory.points[0]);
//;
//display_trajectory.trajectory();
// 00034 , path_tolerance(_alloc)
// 00035 , goal_tolerance(_alloc)
// 00036 , goal_time_tolerance()
//right_arm_planning_controller_pub_.publish(trajectory_controller_msgs);
}
ROS_INFO(" All is DONE !!! ");
}
bool ITRCartesianImpedanceController::init(hardware_interface::PositionCartesianInterface *robot, ros::NodeHandle &n)
{
ROS_INFO("THIS CONTROLLER USES THE TCP INFORMATION SET ON THE FRI SIDE... SO BE SURE YOU KNOW WHAT YOU ARE DOING!");
if (!ros::param::search(n.getNamespace(),"robot_name", robot_namespace_))
{
ROS_WARN_STREAM("ITRCartesianImpedanceController: No robot name found on parameter server ("<<n.getNamespace()<<"/robot_name), using the namespace...");
robot_namespace_ = n.getNamespace();
//return false;
}
if (!n.getParam("robot_name", robot_namespace_))
{
ROS_WARN_STREAM("ITRCartesianImpedanceController: Could not read robot name from parameter server ("<<n.getNamespace()<<"/robot_name), using the namespace...");
robot_namespace_ = n.getNamespace();
//return false;
}
// stuff to read KDL chain in order to get the transform between base_link and robot
if( !(KinematicChainControllerBase<hardware_interface::PositionCartesianInterface >::init(robot, n)) )
{
ROS_ERROR("Couldn't execute init of the KinematikChainController to get the KDL chain.");
return false;
}
KDL::Chain mount_chain;
//cout << "reading segment 0 name:" << endl;
kdl_tree_.getChain(kdl_tree_.getRootSegment()->first, this->robot_namespace_ + "_base_link", mount_chain);
//cout << "KDL segment 0 name: " << mount_chain.getSegment(0).getName() << endl;
base_link2robot_ = mount_chain.getSegment(0).getFrameToTip();
//cout << "base_link2robot transform: " << endl << base_link2robot_ << endl;
joint_names_.push_back( robot_namespace_ + std::string("_0_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_1_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_2_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_3_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_4_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_5_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_6_joint") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_xx") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_yx") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_zx") );
cart_12_names_.push_back( robot_namespace_ + std::string("_pos_x") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_xy") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_yy") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_zy") );
cart_12_names_.push_back( robot_namespace_ + std::string("_pos_y") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_xz") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_yz") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_zz") );
cart_12_names_.push_back( robot_namespace_ + std::string("_pos_z") );
cart_6_names_.push_back( robot_namespace_ + std::string("_X") );
cart_6_names_.push_back( robot_namespace_ + std::string("_Y") );
cart_6_names_.push_back( robot_namespace_ + std::string("_Z") );
cart_6_names_.push_back( robot_namespace_ + std::string("_A") );
cart_6_names_.push_back( robot_namespace_ + std::string("_B") );
cart_6_names_.push_back( robot_namespace_ + std::string("_C") );
// now get all handles, 12 for cart pos, 6 for stiff, 6 for damp, 6 for wrench, 6 for joints
for(int c = 0; c < 30; ++c)
{
if(c < 12)
cart_handles_.push_back(robot->getHandle(cart_12_names_.at(c)));
if(c > 11 && c < 18)
cart_handles_.push_back(robot->getHandle(cart_6_names_.at(c-12) + std::string("_stiffness")));
if(c > 17 && c < 24)
cart_handles_.push_back(robot->getHandle(cart_6_names_.at(c-18) + std::string("_damping")));
if(c > 23 && c < 30)
cart_handles_.push_back(robot->getHandle(cart_6_names_.at(c-24) + std::string("_wrench")));
}
for(int j = 0; j < 6; ++j)
{
joint_handles_.push_back(robot->getHandle(joint_names_.at(j)));
}
sub_pose_ = n.subscribe(n.resolveName("pose"), 1, &ITRCartesianImpedanceController::pose, this);
sub_pose_world_ = n.subscribe(n.resolveName("pose_base_link"), 1, &ITRCartesianImpedanceController::pose_base_link, this);
sub_gains_ = n.subscribe(n.resolveName("gains"), 1, &ITRCartesianImpedanceController::gains, this);
sub_addFT_ = n.subscribe(n.resolveName("wrench"), 1, &ITRCartesianImpedanceController::additionalFT, this);
srv_command_ = n.advertiseService("set_command", &ITRCartesianImpedanceController::command_cb, this);
//sub_ft_measures_ = n.subscribe(n.resolveName("ft_measures"), 1, &ITRCartesianImpedanceController::updateFT, this);
//pub_goal_ = n.advertise<geometry_msgs::PoseStamped>(n.resolveName("goal"),0);
pub_msr_pos_ = n.advertise<geometry_msgs::PoseStamped>(n.resolveName("measured_cartesian_pose"),0);
// Initial position
KDL::Rotation cur_R(cart_handles_.at(0).getPosition(),
cart_handles_.at(1).getPosition(),
cart_handles_.at(2).getPosition(),
cart_handles_.at(4).getPosition(),
cart_handles_.at(5).getPosition(),
cart_handles_.at(6).getPosition(),
cart_handles_.at(8).getPosition(),
cart_handles_.at(9).getPosition(),
cart_handles_.at(10).getPosition());
KDL::Vector cur_p(cart_handles_.at(3).getPosition(),
cart_handles_.at(7).getPosition(),
cart_handles_.at(11).getPosition());
KDL::Frame cur_T( cur_R, cur_p );
x_ref_ = cur_T;
x_des_ = cur_T;
x_set_ = cur_T;
// Initial Cartesian stiffness
KDL::Stiffness k( 800.0, 800.0, 800.0, 50.0, 50.0, 50.0 );
k_des_ = k;
// Initial force/torque measure
KDL::Wrench w(KDL::Vector(0.0, 0.0, 0.0), KDL::Vector(0.0, 0.0, 0.0));
f_des_ = w;
// Initial Cartesian damping
KDL::Stiffness d(0.7, 0.7, 0.7, 0.7, 0.7, 0.7);
d_des_ = d;
for (int i = 0; i < 3; ++i){
if ( !nh_.getParam("position_stiffness_gains", k_des_[i] ) ){
ROS_WARN("Position stiffness gain not set in yaml file, Using %f", k_des_[i]);
}
}
for (int i = 3; i < 6; ++i){
if ( !nh_.getParam("orientation_stiffness_gains", k_des_[i] ) ){
ROS_WARN("Orientation stiffness gain not set in yaml file, Using %f", k_des_[i]);
}
}
for (int i = 0; i < 6; ++i){
if ( !nh_.getParam("damping_gains", d_des_[i]) ){
ROS_WARN("Damping gain not set in yaml file, Using %f", d_des_[i]);
}
}
max_trans_speed_ = 0.1; // m/s
max_rot_speed_ = 0.5; // rad/s
// get params for speed limit
if ( !nh_.getParam("max_trans_speed", max_trans_speed_) ){
ROS_WARN("Max translation speed not set in yaml file, Using %f", max_trans_speed_);
}
if ( !nh_.getParam("max_rot_speed", max_rot_speed_) ){
ROS_WARN("Max rotation speed not set in yaml file, Using %f", max_rot_speed_);
}
std::vector<double> cur_T_FRI;
fromKDLtoFRI(x_des_, cur_T_FRI);
// set initial commands already here:
for(int c = 0; c < 30; ++c)
{
if(c < 12)
cart_handles_.at(c).setCommand(cur_T_FRI.at(c));
if(c > 11 && c < 18)
cart_handles_.at(c).setCommand(k_des_[c-12]);
if(c > 17 && c < 24)
cart_handles_.at(c).setCommand(d_des_[c-18]);
if(c > 23 && c < 30)
cart_handles_.at(c).setCommand(f_des_[c-24]);
}
cmd_flag_ = false;
return true;
}
void ContinuousPolicyLearner::Initialize( ros::NodeHandle& nh,
ros::NodeHandle& ph )
{
WriteLock lock( _mutex );
std::string policyName;
GetParamRequired( ph, "policy_name", policyName );
// Read policy information
if( !_infoManager.CheckMemberInfo( policyName, true, ros::Duration( 10.0 ) ) )
{
throw std::runtime_error( "Could not find policy: " + policyName );
}
const PolicyInfo& info = _infoManager.GetInfo( policyName );
_policy.Initialize( info.inputDim, info.outputDim, info.networkInfo );
// Get initial policy parameters
ros::service::waitForService( info.paramQueryService );
ros::service::waitForService( info.paramSetService );
percepto_msgs::GetParameters::Request req;
percepto_msgs::GetParameters::Response res;
if( !ros::service::call( info.paramQueryService, req, res ) )
{
throw std::runtime_error( "Could not query parameters at: " + info.paramQueryService );
}
_policy.GetParameters()->SetParamsVec( GetVectorView( res.parameters ) );
ROS_INFO_STREAM( "Initialized policy: " << std::endl << *_policy.GetPolicyModule() );
_setParamsClient = nh.serviceClient<percepto_msgs::SetParameters>( info.paramSetService, true );
// Read optimization parameters
ros::NodeHandle lh( ph.resolveName( "optimization" ) );
GetParamRequired( lh, "min_num_modules", _minModulesToOptimize );
GetParam( lh, "clear_optimized_modules", _clearAfterOptimize, false );
if( !_clearAfterOptimize )
{
GetParamRequired( lh, "max_num_modules", _maxModulesToKeep );
}
_optimizer = parse_modular_optimizer( lh );
double l2Weight, maxDivergence;
unsigned int batchSize;
GetParamRequired( lh, "l2_weight", l2Weight );
GetParamRequired( lh, "batch_size", batchSize );
GetParamRequired( lh, "max_divergence", maxDivergence );
_optimization.Initialize( _policy.GetParameters(), l2Weight, batchSize, maxDivergence );
std::string critiqueTopic;
GetParamRequired( ph, "critique_service_topic", critiqueTopic );
ros::service::waitForService( critiqueTopic );
_getCritiqueClient = nh.serviceClient<percepto_msgs::GetCritique>( critiqueTopic, true );
// Subscribe to policy action feed
_actionSub = nh.subscribe( "actions",
0,
&ContinuousPolicyLearner::ActionCallback,
this );
_lastOptimizationTime = ros::Time::now();
double updateRate;
GetParamRequired( lh, "update_rate", updateRate );
_updateTimer = nh.createTimer( ros::Duration( 1.0/updateRate ),
&ContinuousPolicyLearner::TimerCallback,
this );
}
TransformPixel()
{
camerainfosub_ = nh_.subscribe<sensor_msgs::CameraInfo>(nh_.resolveName("camera_info"), 100, &TransformPixel::camerainfoCb, this);
srv_ = nh_.advertiseService("TransformPixelRequest", &TransformPixel::objectSrv,this);
}
static std::string name_snapshot(ros::NodeHandle &node, const std::string &name)
{
return (name == "" ? name::snapshot() : node.resolveName(name) + "_snapshot");
}
