Skip to content

Instantly share code, notes, and snippets.

@PierrickKoch
Created June 3, 2011 07:10
Show Gist options
  • Save PierrickKoch/1005998 to your computer and use it in GitHub Desktop.
Save PierrickKoch/1005998 to your computer and use it in GitHub Desktop.
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE properties SYSTEM "cpf.dtd">
<properties>
<simple name="Import" type="string"><value>rtt_ros_integration_example</value></simple>
<struct name="ROSConTwistOut" type="ConnPolicy">
<simple name="type" type="short"><value>0</value></simple><!-- type of connection: 0 means Data -->
<simple name="size" type="short"><value>1</value></simple><!-- buffer size -->
<simple name="transport" type="short"><value>3</value></simple><!--3 means ROS-->
<simple name="name_id" type="string"><value>/ATRV/Motion_Controller</value></simple><!-- topic name -->
</struct>
<struct name="ROSConLaserIn" type="ConnPolicy">
<simple name="type" type="short"><value>0</value></simple><!-- type of connection: 0 means Data -->
<simple name="size" type="short"><value>1</value></simple><!-- buffer size -->
<simple name="transport" type="short"><value>3</value></simple><!--3 means ROS-->
<simple name="name_id" type="string"><value>/ATRV/Sick</value></simple><!-- topic name -->
</struct>
<struct name="HelloRobot" type="HelloRobotLaser">
<struct name="Activity" type="PeriodicActivity">
<simple name="Period" type="double"><value>1</value></simple>
<simple name="Priority" type="short"><value>0</value></simple>
<simple name="Scheduler" type="string"><value>ORO_SCHED_RT</value></simple>
</struct>
<simple name="AutoConf" type="boolean"><value>1</value></simple>
<simple name="AutoStart" type="boolean"><value>1</value></simple>
<struct name="Ports" type="PropertyBag">
<simple name="twist_out" type="string"><value> ROSConTwistOut </value></simple>
<simple name="laser_in" type="string"><value> ROSConLaserIn </value></simple>
</struct>
</struct>
</properties>
#include <rtt/TaskContext.hpp>
#include <rtt/Port.hpp>
#include <geometry_msgs/Twist.h>
#include <ocl/Component.hpp>
#include <sensor_msgs/LaserScan.h>
#include <numeric>
using namespace std;
using namespace RTT;
class HelloRobotLaser : public RTT::TaskContext {
private:
InputPort<sensor_msgs::LaserScan> inport;
OutputPort<geometry_msgs::Twist> outport;
public:
HelloRobotLaser(const std::string& name):
TaskContext(name),
inport("laser_in"),
outport("twist_out")
{
ports()->addPort(inport);
ports()->addPort(outport);
}
~HelloRobotLaser() {}
private:
void updateHook() {
sensor_msgs::LaserScan msg;
if (NewData == inport.read(msg)) {
geometry_msgs::Twist cmd;
bool halt = false;
for (int i = 75; i <= 105; i++) {
if (msg.ranges[i] < 2.0) {
halt = true;
break;
}
}
if (halt) {
double midA, midB;
cmd.linear.x = 0;
// we go to the highest-range side scanned
midA = std::accumulate(msg.ranges.begin(), msg.ranges.end()-90, 0);
midB = std::accumulate(msg.ranges.begin()+90, msg.ranges.end(), 0);
log(Info)<<"A:"<<midA<<", B:"<<midB<<endlog();
if (midA > midB) {
cmd.angular.z = -1;
} else {
cmd.angular.z = 1;
}
} else {
cmd.linear.x = 1;
}
outport.write(cmd);
}
}
};
ORO_CREATE_COMPONENT(HelloRobotLaser)
<package>
<description brief="rtt_ros_integration_example">
<include virtual="doc.html"/>
</description>
<author>Ruben Smits, [email protected]</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/rtt_ros_integration_example</url>
<depend package="rtt_ros_integration"/>
<depend package="rtt_ros_integration_std_msgs"/>
<depend package="rtt_ros_integration_geometry_msgs"/>
<depend package="rtt_ros_integration_sensor_msgs"/>
<depend package="rtt_ros_param"/>
<depend package="rtt"/>
</package>
from morse.builder.morsebuilder import *
# Append ATRV robot to the scene
atrv = Robot('atrv')
# Append an actuator
motion = Controller('morse_vw_control')
motion.translate(z=0.3)
atrv.append(motion)
# Append a sick laser
sick = Sensor('morse_sick')
sick.translate(x=0.18,z=0.94)
atrv.append(sick)
sick.properties(resolution = 1)
# Insert the middleware object
ros = Middleware('ros_empty')
# Configure the middlewares
ros.configure(motion)
ros.configure(sick)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment