#! /usr/bin/env python
import re
class AcpiChecker():
"""read /proc/acpi/battery/*** files for check the PC's battery"""
def __init__(self, battery_name):
self._info_path = "/proc/acpi/battery/" + battery_name + "/info"
self._state_path = "/proc/acpi/battery/" + battery_name + "/state"
def get_capacity(self):
f = open(self._info_path)
for line in f:
match = re.search(r'^last full capacity:\s*(\d+)', line)
if match:
capacity = match.groups()[0]
f.close()
return capacity
def get_state(self):
f = open(self._state_path)
for line in f:
match = re.search(r'^remaining capacity:\s*(\d+)', line)
if match:
state = match.groups()[0]
f.close()
return state
def get_rate(self):
return float(self.get_state()) / float(self.get_capacity())
if __name__=='__main__':
checker = AcpiChecker('BAT0')
print checker.get_rate()
#! /usr/bin/env python
import roslib
roslib.load_manifest('otl_battery_checker')
import rospy
from std_msgs.msg import Float64
from battery_check import AcpiChecker
class BatteryCheckerNode():
"""check battery and publish the charge rate"""
def __init__(self, battery_name='BAT0'):
self._checker = AcpiChecker(battery_name)
self._pub = rospy.Publisher('/pc/battery_rate', Float64)
def proc(self):
msg = Float64()
msg.data = self._checker.get_rate()
self._pub.publish(msg)
if __name__=='__main__':
rospy.init_node('pc_battery_checker')
battery_name = 'BAT0'
if rospy.has_param('~device'):
battery_name = rospy.get_param('~device')
check = BatteryCheckerNode(battery_name)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
check.proc()
rate.sleep()
<launch> <param name="robot_description" textfile="$(find hoge_description)/urdf/hoge.xml"/> <param name="use_gui" value="true"/> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /> <node name="rviz" pkg="rviz" type="rviz"/> </launch>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/cloud_viewer.h>
int
main (int argc, char** argv)
{
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
reader.read ("table_scene_lms400.pcd", *cloud);
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01, 0.01, 0.01);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud_filtered);
seg.segment (*inliers, *coefficients); //*
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Write the planar inliers to disk
extract.filter (*cloud_plane); //*
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_filtered); //*
}
// Creating the KdTree object for the search method of the extraction
pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud( cloud_filtered);
ec.extract (cluster_indices);
int j = 0;
float colors[6][3] ={{255, 0, 0}, {0,255,0}, {0,0,255}, {255,255,0}, {0,255,255}, {255,0,255}};
pcl::visualization::CloudViewer viewer("cluster viewer");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud_filtered, *cloud_cluster);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) {
cloud_cluster->points[*pit].r = colors[j%6][0];
cloud_cluster->points[*pit].g = colors[j%6][1];
cloud_cluster->points[*pit].b = colors[j%6][2];
}
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZRGB> (ss.str (), *cloud_cluster, false); //*
j++;
}
viewer.showCloud (cloud_cluster);
while (!viewer.wasStopped())
{
sleep (1);
}
return (0);
}
// pack r/g/b into rgb
uint8_t r = 255, g = 0, b = 0; // Example: Red color
uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
p.rgb = *reinterpret_cast<float*>(&rgb);
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGB> cloud;
// Fill in the cloud data
cloud.width = 15;
cloud.height = 10;
cloud.points.resize (cloud.width * cloud.height);
// Generate the data
size_t i;
for (i = 0; i < cloud.points.size ()/2; ++i)
{
// 平面におく
cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
cloud.points[i].z = 0.1 * rand () / (RAND_MAX + 1.0);
cloud.points[i].r = 255;
cloud.points[i].g = 255;
cloud.points[i].b = 255;
}
for (; i < cloud.points.size (); ++i)
{
// ちらばらせる
cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
cloud.points[i].r = 255;
cloud.points[i].g = 255;
cloud.points[i].b = 255;
}
// Set a few outliers
cloud.points[0].z = 2.0;
cloud.points[3].z = -2.0;
cloud.points[6].z = 4.0;
std::cerr << "Point cloud data: " << cloud.points.size () << " points" << std::endl;
for (size_t i = 0; i < cloud.points.size (); ++i)
std::cerr << " " << cloud.points[i].x << " "
<< cloud.points[i].y << " "
<< cloud.points[i].z << std::endl;
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.1);
seg.setInputCloud (cloud.makeShared ());
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model for the given dataset.");
return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
for (size_t i = 0; i < inliers->indices.size (); ++i) {
std::cerr << inliers->indices[i] << " " << cloud.points[inliers->indices[i]].x << " "
<< cloud.points[inliers->indices[i]].y << " "
<< cloud.points[inliers->indices[i]].z << std::endl;
cloud.points[inliers->indices[i]].r = 255;
cloud.points[inliers->indices[i]].g = 0;
cloud.points[inliers->indices[i]].b = 0;
}
pcl::visualization::CloudViewer viewer("Cloud Viewer");
viewer.showCloud(cloud.makeShared());
while (!viewer.wasStopped ())
{
}
return (0);
}
するとこんな感じで平面っぽいところにあるポイントが赤く表示されました。
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>
void segmentate(pcl::PointCloud<pcl::PointXYZRGB>& cloud, double threshould) {
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (threshould);
seg.setInputCloud (cloud.makeShared ());
seg.segment (*inliers, *coefficients);
for (size_t i = 0; i < inliers->indices.size (); ++i) {
cloud.points[inliers->indices[i]].r = 255;
cloud.points[inliers->indices[i]].g = 0;
cloud.points[inliers->indices[i]].b = 0;
}
}
class SimpleOpenNIViewer
{
public:
SimpleOpenNIViewer () : viewer ("PCL OpenNI Viewer") {}
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud)
{
if (!viewer.wasStopped()) {
pcl::PointCloud<pcl::PointXYZRGB> segmented_cloud(*cloud);
segmentate(segmented_cloud, 0.01);
viewer.showCloud (segmented_cloud.makeShared());
}
}
void run ()
{
pcl::Grabber* interface = new pcl::OpenNIGrabber();
boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f =
boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);
interface->registerCallback (f);
interface->start ();
while (!viewer.wasStopped())
{
sleep (1);
}
interface->stop ();
}
pcl::visualization::CloudViewer viewer;
};
int main ()
{
SimpleOpenNIViewer v;
v.run ();
return 0;
}#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
class SimpleOpenNIViewer
{
public:
SimpleOpenNIViewer () : viewer ("PCL OpenNI Viewer") {}
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud)
{
if (!viewer.wasStopped())
viewer.showCloud (cloud);
}
void run ()
{
pcl::Grabber* interface = new pcl::OpenNIGrabber();
boost::function<void (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f =
boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);
interface->registerCallback (f);
interface->start ();
while (!viewer.wasStopped())
{
sleep (1);
}
interface->stop ();
}
pcl::visualization::CloudViewer viewer;
};
int main ()
{
SimpleOpenNIViewer v;
v.run ();
return 0;
}
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h> // 追加
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ> cloud;
// Fill in the cloud data
cloud.width = 5;
cloud.height = 1;
cloud.is_dense = false;
cloud.points.resize (cloud.width * cloud.height);
for (size_t i = 0; i < cloud.points.size (); ++i)
{
cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
}
// 追加
pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
viewer.showCloud (cloud.makeShared());
while (!viewer.wasStopped ()) {
}
return (0);
}
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(pcd_write)
find_package(PCL 1.1 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (pcd_write pcd_write.cpp)
add_executable (pcl_view_test pcl_view_test.cpp)
target_link_libraries (pcd_write ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES})
target_link_libraries (pcl_view_test ${PCL_LIBRARIES})
sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl sudo apt-get update sudo apt-get install libpcl-1.1-dev
$ cmake .とするとpcd_writeという実行ファイルができるはずです。
$ make
$ ./pcd_writeして、以下のように表示されればOK
$ ./pcd_write
Saved 5 data points to test_pcd.pcd.
0.352222 -0.151883 -0.106395
-0.397406 -0.473106 0.292602
-0.731898 0.667105 0.441304
-0.734766 0.854581 -0.0361733
-0.4607 -0.277468 -0.916762
# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 5
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 5
DATA ascii
0.35222197 -0.15188313 -0.10639524
-0.3974061 -0.47310591 0.29260206
-0.73189831 0.66710472 0.44130373
-0.73476553 0.85458088 -0.036173344
-0.46070004 -0.2774682 -0.91676188
// -*- C++ -*-
/*!
* @file JoyRTM2velROS.h
* @brief convert Joystick sample port to ROS twist msg
* @date $Date$
*
* $Id$
*/
#ifndef JOYRTM2VELROS_H
#define JOYRTM2VELROS_H
#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>
#include <rtm/idl/ExtendedDataTypesSkel.h>
#include <rtm/idl/InterfaceDataTypesSkel.h>
#include <rtm/RosOutPort.h>
#include "geometry_msgs/Twist.h"
using namespace RTC;
/*!
* @class JoyRTM2velROS
* @brief convert Joystick sample port to ROS twist msg
*
*/
class JoyRTM2velROS
: public RTC::DataFlowComponentBase
{
public:
/*!
* @brief constructor
* @param manager Maneger Object
*/
JoyRTM2velROS(RTC::Manager* manager);
/*!
* @brief destructor
*/
~JoyRTM2velROS();
/***
*
* The initialize action (on CREATED->ALIVE transition)
* formaer rtc_init_entry()
*
* @return RTC::ReturnCode_t
*
*
*/
virtual RTC::ReturnCode_t onInitialize();
/***
*
* The execution action that is invoked periodically
* former rtc_active_do()
*
* @param ec_id target ExecutionContext Id
*
* @return RTC::ReturnCode_t
*
*
*/
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
protected:
// Configuration variable declaration
//
/*!
*
* - Name: max_linear_velocity
* - DefaultValue: 0.1
*/
double m_max_linear_velocity;
/*!
*
* - Name: max_angular_velocity
* - DefaultValue: 0.2
*/
double m_max_angular_velocity;
//
// DataInPort declaration
//
TimedFloatSeq m_joyInput;
/*!
*/
InPort m_joyInputIn;
// ROS Port
geometry_msgs::Twist m_rosVel;
RosOutPort m_rosVelOut;
private:
};
extern "C"
{
DLL_EXPORT void JoyRTM2velROSInit(RTC::Manager* manager);
};
#endif // JOYRTM2VELROS_H
// -*- C++ -*-
/*!
* @file JoyRTM2velROS.cpp
* @brief convert Joystick sample port to ROS twist msg
* @date $Date$
*
* $Id$
*/
#include "JoyRTM2velROS.h"
// Module specification
// <rtc-template block="module_spec">
static const char* joyrtm2velros_spec[] =
{
"implementation_id", "JoyRTM2velROS",
"type_name", "JoyRTM2velROS",
"description", "convert Joystick sample port to ROS twist msg",
"version", "1.0.0",
"vendor", "OTL",
"category", "Sample",
"activity_type", "PERIODIC",
"kind", "DataFlowComponent",
"max_instance", "1",
"language", "C++",
"lang_type", "compile",
// Configuration variables
"conf.default.max_linear_velocity", "0.2",
"conf.default.max_angular_velocity", "0.4",
// Widget
"conf.__widget__.max_linear_velocity", "slider",
"conf.__widget__.max_angular_velocity", "slider",
// Constraints
""
};
// </rtc-template>
/*!
* @brief constructor
* @param manager Maneger Object
*/
JoyRTM2velROS::JoyRTM2velROS(RTC::Manager* manager)
// <rtc-template block="initializer">
: RTC::DataFlowComponentBase(manager),
m_joyInputIn("joy_input", m_joyInput)
// </rtc-template>
,
m_rosVelOut("cmd_vel", "rtm2ros", m_rosVel)
{
}
/*!
* @brief destructor
*/
JoyRTM2velROS::~JoyRTM2velROS()
{
}
RTC::ReturnCode_t JoyRTM2velROS::onInitialize()
{
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("joy_input", m_joyInputIn);
// Set OutPort buffer
// Set service provider to Ports
// Set service consumers to Ports
// Set CORBA Service Ports
// </rtc-template>
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("max_linear_velocity", m_max_linear_velocity, "0.2");
bindParameter("max_angular_velocity", m_max_angular_velocity, "0.4");
// </rtc-template>
addRosOutPort("ros_velocity", m_rosVelOut);
return RTC::RTC_OK;
}
RTC::ReturnCode_t JoyRTM2velROS::onExecute(RTC::UniqueId ec_id)
{
if(m_joyInputIn.isNew()) {
m_joyInputIn.read();
m_rosVel.linear.x = m_joyInput.data[1] * 0.005;
m_rosVel.angular.z = -m_joyInput.data[0] * 0.010;
if ( m_max_linear_velocity < m_rosVel.linear.x) {
m_rosVel.linear.x = m_max_linear_velocity;
} else if (-m_max_linear_velocity > m_rosVel.linear.x) {
m_rosVel.linear.x = -m_max_linear_velocity;
} else if (fabs(m_rosVel.linear.x) < 0.01) {
m_rosVel.linear.x = 0.0;
}
if ( m_max_angular_velocity < m_rosVel.angular.z) {
m_rosVel.angular.z = m_max_angular_velocity;
} else if (-m_max_angular_velocity > m_rosVel.angular.z) {
m_rosVel.angular.z = -m_max_angular_velocity;
} else if (fabs(m_rosVel.angular.z) < 0.01) {
m_rosVel.angular.z = 0.0;
}
m_rosVelOut.write();
}
return RTC::RTC_OK;
}
extern "C"
{
void JoyRTM2velROSInit(RTC::Manager* manager)
{
coil::Properties profile(joyrtm2velros_spec);
manager->registerFactory(profile,
RTC::Create<JoyRTM2velROS>,
RTC::Delete<JoyRTM2velROS>);
}
};
