Commit 209077e6 authored by Simon M. Haller-Seeber's avatar Simon M. Haller-Seeber
Browse files

removed unnecassary files

parent 0642c568
......@@ -103,19 +103,6 @@ target_link_libraries(sphero_rvr_hw_node sphero_rvr_hw_interface ${catkin_LIBRAR
add_executable(base_controller ${PROJECT_ROOT}/src/base_controller.cpp)
target_link_libraries(base_controller rvr++ ${catkin_LIBRARIES})
#add_dependencies(base_controller rvr_hardware_interface_generate_messages_cpp)
#add_executable(tf_broadcaster ${PROJECT_ROOT}/src/tf_broadcaster.cpp)
#target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})
#add_executable(tf_listener ${PROJECT_ROOT}/src/tf_listener.cpp)
#target_link_libraries(tf_listener ${catkin_LIBRARIES})
#add_executable(laser ${PROJECT_ROOT}/src/laser.cpp)
#target_link_libraries(laser ${catkin_LIBRARIES})
#add_executable(odometry src/odometry.cpp)
#target_link_libraries(odometry ${catkin_LIBRARIES})
#############
## Install ##
......
......@@ -109,7 +109,7 @@ async def write_motors(l_mode, l_speed, r_mode, r_speed):
def cmd_vel_callback(cmdtwist):
print("got something")
#print("got something")
v_x = cmdtwist.linear.x
v_th = cmdtwist.angular.z
......@@ -155,7 +155,7 @@ def check_if_need_to_send_msg(component):
'/sphero_rvr_base_link',
'/odom'
)
print("published")
#print("published")
except Exception:
traceback.print_exc()
......@@ -189,7 +189,7 @@ async def velocity_handler(velocity_data):
async def handle_ros():
print("Handle ROS")
#print("Handle ROS")
await asyncio.sleep(0.1)
async def rvr_robot():
......
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "laser_scan_publisher");
ros::NodeHandle n;
ros::Publisher publisher = n.advertise<sensor_msgs::LaserScan>("scan", 50);
unsigned int points = 100;
double laser_freq = 40;
double ranges[points];
double intensities[points];
int count = 0;
ros::Rate r(1.0);
while ( n.ok() )
{
for (unsigned int i = 0; i < points; ++i) {
ranges[i] = count;
intensities[i] = 100 + count;
}
ros::Time scan_time = ros::Time::now();
sensor_msgs::LaserScan scan;
scan.header.stamp = scan_time;
scan.header.frame_id = "base_link";
scan.angle_min = -1.57;
scan.angle_max = 1.57;
scan.angle_increment = 3.14 / points;
scan.time_increment = (1 / laser_freq) / points;
scan.range_min = 0.0;
scan.range_max = 100.0;
scan.ranges.resize(points);
scan.intensities.resize(points);
for (unsigned int i = 0; i < points; ++i) {
scan.ranges[i] = ranges[i];
scan.intensities[i] = intensities[i];
}
publisher.publish(scan);
++count;
r.sleep();
}
}
int main(int argc, char** argv)
{
while ( ros::ok() )
{
time = ros::Time::now();
double dt = ( time - time_last ).toSec();
double delta_x = ( vx * cos(th) - vy * sin(th) ) * dt;
double delta_y = ( vx * sin(th) + vy * cos(th) ) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th += delta_th;
geometry_msgs::Quaternion odom_quat;
odom_quat = tf::createQuaternionMsgFromRollPitchYaw( 0, 0, th );
odom_trans.header.stamp = time;
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw( th );
nav_msgs::Odometry odom;
odom.header.stamp = time;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.linear.z = 0.0;
odom.twist.twist.angular.x = 0.0;
odom.twist.twist.angular.y = 0.0;
odom.twist.twist.angular.z = vth;
time_last = time;
broadcaster.sendTransform( odom_trans );
publisher.publish( odom );
loop_rate.sleep();
}
return 0;
}
#include <ros/ros.h>
int main()
{
ROS_INFO_STREAM("Hello, World!");
}
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "rvr_tf_broadcaster");
ros::NodeHandle n;
ros::Rate r(100);
tf::TransformBroadcaster broadcaster;
while ( n.ok() )
{
broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
ros::Time::now(), "base_link", "base_laser"));
r.sleep();
}
}
#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>
void transform_point(const tf::TransformListener& listener)
{
geometry_msgs::PointStamped point;
point.header.frame_id = "base_point";
point.header.stamp = ros::Time();
point.point.x = 0.0;
point.point.y = 1.0;
point.point.z = 2.0;
geometry_msgs::PointStamped base_point;
listener.transformPoint("base_link", point, base_point);
ROS_INFO("point: (%.2f, %.2f, %.2f) -> base_link: (%.2f, %.2f, %.2f) at time %.2f",
point.point.x, point.point.y, point.point.z,
base_point.point.x, base_point.point.y, base_point.point.z,
base_point.header.stamp.toSec()
);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "rvr_tf_listener");
ros::NodeHandle n;
tf::TransformListener listener(ros::Duration(10));
ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transform_point, boost::ref(listener)));
ros::spin();
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment