GripperRosControl(const std::vector<std::string>& actr_names, const std::vector<std::string>& jnt_names,
const std::vector<std::string>& controller_names, const std::vector<double>& reducers)
: actr_names_(actr_names)
, jnt_names_(jnt_names)
, controller_names_(controller_names)
{
for (int i = 0; i < jnt_names_.size(); i++)
{
std::string actr = actr_names_[i];
std::string jnt = jnt_names_[i];
// Get transmission
boost::shared_ptr<transmission_interface::SimpleTransmission> t_ptr(
new transmission_interface::SimpleTransmission(reducers[i]));
trans_.push_back(t_ptr);
// Initialize and wrap raw current data
actr_curr_data_[actr].position.push_back(&actr_curr_pos_[actr]);
actr_curr_data_[actr].velocity.push_back(&actr_curr_vel_[actr]);
actr_curr_data_[actr].effort.push_back(&actr_curr_eff_[actr]);
jnt_curr_data_[jnt].position.push_back(&jnt_curr_pos_[jnt]);
jnt_curr_data_[jnt].velocity.push_back(&jnt_curr_vel_[jnt]);
jnt_curr_data_[jnt].effort.push_back(&jnt_curr_eff_[jnt]);
// Initialize and wrap raw command data
actr_cmd_data_[actr].position.push_back(&actr_cmd_pos_[actr]);
jnt_cmd_data_[jnt].position.push_back(&jnt_cmd_pos_[jnt]);
// Register transmissions to each interface
actr_to_jnt_state_.registerHandle(transmission_interface::ActuatorToJointStateHandle(
actr + "_trans", trans_[i].get(), actr_curr_data_[actr], jnt_curr_data_[jnt]));
jnt_to_actr_pos_.registerHandle(transmission_interface::JointToActuatorPositionHandle(
jnt + "_trans", trans_[i].get(), actr_cmd_data_[actr], jnt_cmd_data_[jnt]));
// Connect and register the joint state handle
hardware_interface::JointStateHandle state_handle(jnt, &jnt_curr_pos_[jnt], &jnt_curr_vel_[jnt],
&jnt_curr_eff_[jnt]);
jnt_state_interface_.registerHandle(state_handle);
// Connect and register the joint position handle
hardware_interface::JointHandle pos_handle(jnt_state_interface_.getHandle(jnt), &jnt_cmd_pos_[jnt]);
pos_jnt_interface_.registerHandle(pos_handle);
// ROS publishers and subscribers
actr_cmd_pub_[actr] = nh_.advertise<std_msgs::Float64>("dxl/" + controller_names_[i] + "/command", 5);
actr_state_sub_[actr] =
nh_.subscribe("dxl/" + controller_names_[i] + "/state", 1, &GripperRosControl::actrStateCallback, this);
}
// Register interfaces
registerInterface(&jnt_state_interface_);
registerInterface(&pos_jnt_interface_);
// Start spinning
nh_.setCallbackQueue(&subscriber_queue_);
subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_));
subscriber_spinner_->start();
}
ImageConverter()
: it_(nh_),
ch_(nh_, "trigger_node")
{
ch_.setCallbackQueue(&trigger_queue_);
// Subscribe to input video feed and publish output video feed
image_sub_ = it_.subscribe("/camera/image_raw", 1,
&ImageConverter::imageCb, this);
image_pub_ = it_.advertise("/image_converter/output_video", 1);
cv::namedWindow(OPENCV_WINDOW);
advertisetriggerservice("no_name");
}
/**
* Initialize module - called in server constructor
*/
void srs_env_model::COcPatchMaker::init(ros::NodeHandle & node_handle)
{
ROS_DEBUG("Initializing CCompressedPointCloudPlugin");
if ( m_bSpinThread )
{
// if we're spinning our own thread, we'll also need our own callback queue
node_handle.setCallbackQueue( &callback_queue_ );
need_to_terminate_ = false;
spin_thread_.reset( new boost::thread(boost::bind(&COcPatchMaker::spinThread, this)) );
node_handle_ = node_handle;
}
// Read parameters
{
// Where to get camera position information
node_handle.param("camera_info_topic_name", m_cameraInfoTopic, CPC_CAMERA_INFO_PUBLISHER_NAME );
}
// Subscribe to position topic
// Create subscriber
m_camPosSubscriber = node_handle.subscribe<sensor_msgs::CameraInfo>( m_cameraInfoTopic, 10, &srs_env_model::COcPatchMaker::onCameraChangedCB, this );
if (!m_camPosSubscriber)
{
ROS_ERROR("Not subscribed...");
ROS_ERROR( "Cannot subscribe to the camera position publisher..." );
}
// stereo cam params for sensor cone:
// node_handle.param<int> ("compressed_pc_camera_stereo_offset_left", m_camera_stereo_offset_left, 0); // 128
// node_handle.param<int> ("compressed_pc_camera_stereo_offset_right", m_camera_stereo_offset_right, 0);
node_handle.param("registration_patch_view_fraction_x", m_fracX, m_fracX );
node_handle.param("registration_patch_view_fraction_y", m_fracY, m_fracY );
node_handle.param("registration_patch_publish_cloud", m_bPublishCloud, m_bPublishCloud );
if( m_bPublishCloud )
// Create publisher - simple point cloud
m_pubConstrainedPC = node_handle.advertise<sensor_msgs::PointCloud2> (REGISTRATION_CONSTRAINED_CLOUD_PUBLISHER_NAME, 5, false);
}
void srs::CLimitedPointCloudPlugin::init(ros::NodeHandle & node_handle)
{
if ( m_bSpinThread )
{
// if we're spinning our own thread, we'll also need our own callback queue
node_handle.setCallbackQueue( &callback_queue_ );
need_to_terminate_ = false;
spin_thread_.reset( new boost::thread(boost::bind(&CLimitedPointCloudPlugin::spinThread, this)) );
node_handle_ = node_handle;
}
// Read parameters
// Camera position topic name
std::string cameraPositionTopic;
node_handle.param("rviz_camera_position_topic_name", cameraPositionTopic, CAMERA_POSITION_TOPIC_NAME );
// Point cloud publishing topic name
node_handle.param("pointcloud_centers_publisher", m_pcPublisherName, POINTCLOUD_CENTERS_PUBLISHER_NAME );
// Create publisher
m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 100, m_latchedTopics);
// Subscribe to position topic
// Create subscriber
m_camPosSubscriber = node_handle.subscribe<srs_env_model_msgs::RVIZCameraPosition>( cameraPositionTopic, 10, &srs::CLimitedPointCloudPlugin::onCameraPositionChangedCB, this );
// = new message_filters::Subscriber<srs_env_model_msgs::RVIZCameraPosition>(node_handle, cameraPositionTopic, 1);
if (!m_camPosSubscriber)
{
ROS_ERROR("Not subscribed...");
PERROR( "Cannot subscribe to the camera position publisher..." );
}
/*
// Create message filter
m_tfCamPosSub = new tf::MessageFilter<srs_env_model_msgs::RVIZCameraPosition>( *m_camPosSubscriber, m_tfListener, "/map", 1);
m_tfCamPosSub->registerCallback(boost::bind( &CLimitedPointCloudPlugin::onCameraPositionChangedCB, this, _1));
*/
// Clear old pointcloud data
m_data->clear();
}
ShowTFTree() : root_nh_(""), priv_nh_("~")
{
tf_counter_ = 0;
// subscribe to /tf
string sub_topic;
priv_nh_.param<string>("subscribe_topic", sub_topic, "/tf");
sub_ = root_nh_.subscribe(sub_topic, 10, &ShowTFTree::callback, this);
root_nh_.setCallbackQueue(&tf_message_callback_queue_);
// create thread for processing callback queue
boost::thread* dedicated_listener_thread_;
dedicated_listener_thread_ = new boost::thread(boost::bind(&ShowTFTree::processCallbackQueueThread, this));
// waiting callback
cout <<"Collecting tf frames.. " ;
wait_time_ = 3;
priv_nh_.getParam("wait", wait_time_);
sync_timer_ = timer_nh_.createTimer(ros::Duration(1.0), boost::bind( &ShowTFTree::wait, this ) );
}
/**
* Initialize plugin
*/
void srs_env_model::CCompressedPointCloudPlugin::init(ros::NodeHandle & node_handle)
{
ROS_DEBUG("Initializing CCompressedPointCloudPlugin");
// 2013/01/31 Majkl: I guess we should publish the map in the Octomap TF frame...
node_handle.param("ocmap_frame_id", m_frameId, m_frameId);
if ( m_bSpinThread )
{
// if we're spinning our own thread, we'll also need our own callback queue
node_handle.setCallbackQueue( &callback_queue_ );
need_to_terminate_ = false;
spin_thread_.reset( new boost::thread(boost::bind(&CCompressedPointCloudPlugin::spinThread, this)) );
node_handle_ = node_handle;
}
// Read parameters
{
// Where to get camera position information
node_handle.param("compressed_pc_camera_info_topic_name", m_cameraInfoTopic, CPC_CAMERA_INFO_PUBLISHER_NAME );
ROS_INFO("SRS_ENV_SERVER: parameter - compressed_pc_camera_info_topic_name: %s", m_cameraInfoTopic.c_str() );
// Point cloud publishing topic name
node_handle.param("compressed_pc_pointcloud_centers_publisher", m_pcPublisherName, CPC_SIMPLE_PC_PUBLISHING_TOPIC_NAME);
ROS_INFO("SRS_ENV_SERVER: parameter - compressed_pc_pointcloud_centers_publisher: %s", m_pcPublisherName.c_str() );
// Should simple pointcloud be published too?
node_handle.param("publish_simple_cloud", m_bPublishSimpleCloud, false );
// How many uncomplete frames should be published before complete one
int uf;
node_handle.param("compressed_pc_differential_frames_count", uf, CPC_NUM_DIFFERENTIAL_FRAMES );
m_uncomplete_frames = uf;
ROS_INFO("SRS_ENV_SERVER: parameter - compressed_pc_update_data_topic_name: %i", uf );
// Complete data topic name
node_handle.param( "compressed_pc_update_data_topic_name", m_ocUpdatePublisherName, CPC_COMPLETE_TOPIC_NAME );
ROS_INFO("SRS_ENV_SERVER: parameter - compressed_pc_update_data_topic_name: %s", m_ocUpdatePublisherName.c_str() );
}
// Services
{
m_serviceSetNumIncomplete = node_handle.advertiseService( SetNumIncompleteFrames_SRV,
&srs_env_model::CCompressedPointCloudPlugin::setNumIncompleteFramesCB, this );
}
// Create publisher - simple point cloud
if( m_bPublishSimpleCloud)
m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 5, m_latchedTopics);
// Create publisher - packed info - cam position and update pointcloud
m_ocUpdatePublisher = node_handle.advertise< srs_env_model::OctomapUpdates > ( m_ocUpdatePublisherName, 5, m_latchedTopics );
// Subscribe to position topic
// Create subscriber
m_camPosSubscriber = node_handle.subscribe<sensor_msgs::CameraInfo>( m_cameraInfoTopic, 10, &srs_env_model::CCompressedPointCloudPlugin::onCameraChangedCB, this );
// = new message_filters::Subscriber<srs_env_model_msgs::RVIZCameraPosition>(node_handle, cameraPositionTopic, 1);
if (!m_camPosSubscriber)
{
ROS_ERROR("Not subscribed...");
PERROR( "Cannot subscribe to the camera position publisher..." );
}
/*
// Create message filter
m_tfCamPosSub = new tf::MessageFilter<srs_env_model_msgs::RVIZCameraPosition>( *m_camPosSubscriber, m_tfListener, "/map", 1);
m_tfCamPosSub->registerCallback(boost::bind( &CCompressedPointCloudPlugin::onCameraPositionChangedCB, this, _1));
*/
// Clear old pointcloud data
m_data->clear();
// stereo cam params for sensor cone:
node_handle.param<int> ("compressed_pc_camera_stereo_offset_left", m_camera_stereo_offset_left, 0); // 128
node_handle.param<int> ("compressed_pc_camera_stereo_offset_right", m_camera_stereo_offset_right, 0);
}