L1.5/ros2aria
L1.5/ros2aria
7
0
Fork 0
Code Issues 5 Pull Requests Projects 1 Releases Wiki Activity

Compare commits

base: L1.5:main
Branches Tags
L1.5:main
L1.5:humble-cyclonedds
L1.5:humble
L1.5:humble-powerbot
L1.5:old
L1.5:pose
..
compare: L1.5:old
Branches Tags
L1.5:humble-cyclonedds
L1.5:humble
L1.5:humble-powerbot
L1.5:old
L1.5:main
L1.5:pose

1 Commits
main ... old

Author SHA1 Message Date
Jakub Delicat
180637b2d0 old changes 2022-07-29 11:44:09 +00:00
22 changed files with 1189 additions and 160 deletions
Show Stats Expand all files Collapse all files

4
Dockerfile.devcontainer
View File

@ -4,7 +4,7 @@ FROM athackst/ros2:foxy-dev
# #
ENV DEBIAN_FRONTEND=noninteractive ENV DEBIAN_FRONTEND=noninteractive
RUN apt-get update \ RUN apt-get update \
&& apt-get -y install --no-install-recommends rsync \ && apt-get -y install --no-install-recommends rsync ros-foxy-xacro ros-foxy-joint-state-publisher-gui ros-foxy-urg-node\
# #
# Clean up # Clean up
&& apt-get autoremove -y \ && apt-get autoremove -y \
@ -26,4 +26,4 @@ RUN SNIPPET="export PROMPT_COMMAND='history -a' && export HISTFILE=/commandhisto
&& mkdir /commandhistory \ && mkdir /commandhistory \
&& touch /commandhistory/.bash_history \ && touch /commandhistory/.bash_history \
&& chown -R $USERNAME /commandhistory \ && chown -R $USERNAME /commandhistory \
&& echo $SNIPPET >> "/home/$USERNAME/.bashrc" && echo $SNIPPET >> "/home/$USERNAME/.bashrc"

2
Makefile
View File

@ -16,7 +16,7 @@ upload:
# upload: .uploaded # upload: .uploaded
run: upload run: upload
ssh lab1_5@pionier6 -t -- docker run --rm --network=host -it --device /dev/ttyS0 -v /home/lab1_5:/ws irth7/ros2aria-dev /bin/bash /ws/ros2aria/run.sh ssh lab1_5@pionier6 -t -- docker run --rm --net host -it --device /dev/ttyS0 -v /home/lab1_5:/ws -v /dev/shm:/dev/shm irth7/ros2aria-dev /bin/bash /ws/ros2aria/run.sh
legacy: legacy:
ssh lab1_5@pionier6 -t -- ./run.sh ssh lab1_5@pionier6 -t -- ./run.sh

40
src/pionier_description/CMakeLists.txt Normal file
View File

@ -0,0 +1,40 @@
cmake_minimum_required(VERSION 3.5)
project(pionier_description)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
install(
DIRECTORY launch urdf meshes
DESTINATION share/${PROJECT_NAME}
)
ament_package()

57
src/pionier_description/launch/pionier_description_bringup.launch.py Normal file
View File

@ -0,0 +1,57 @@
import launch
from launch.substitutions import Command, LaunchConfiguration
import launch_ros
import os
from launch_ros.actions import Node
def generate_launch_description():
pkg_share = launch_ros.substitutions.FindPackageShare(package='pionier_description').find('pionier_description')
default_model_path = os.path.join(pkg_share, 'urdf/description.urdf.xacro')
# default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz')
robot_state_publisher_node = launch_ros.actions.Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
)
joint_state_publisher_node = launch_ros.actions.Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
)
joint_state_publisher_gui_node = launch_ros.actions.Node(
package='joint_state_publisher',
executable='joint_state_publisher',
name='joint_state_publisher',
# condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
)
# rviz_node = launch_ros.actions.Node(
# package='rviz2',
# executable='rviz2',
# name='rviz2',
# output='screen',
# arguments=['-d', LaunchConfiguration('rvizconfig')],
# )
urg_node = Node(
package='urg_node',
executable='urg_node_driver',
emulate_tty=True,
parameters=[{'serial_port': '/dev/ttyACM0'}],
output='screen'
)
return launch.LaunchDescription([
urg_node,
launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
description='Flag to enable joint_state_publisher_gui'),
launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
description='Absolute path to robot urdf file'),
# launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
# description='Absolute path to rviz config file'),
joint_state_publisher_node,
joint_state_publisher_gui_node,
robot_state_publisher_node,
# rviz_node
])

731
src/pionier_description/meshes/d435.dae Normal file
View File

File diff suppressed because one or more lines are too long

18
src/pionier_description/package.xml Normal file
View File

@ -0,0 +1,18 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>pionier_description</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="root@todo.todo">root</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

164
src/pionier_description/urdf/description.urdf.xacro Normal file
View File

@ -0,0 +1,164 @@
<?xml version="1.0"?>
<robot name="sam_bot" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Define robot constants -->
<xacro:property name="base_width" value="0.381"/>
<xacro:property name="base_length" value="0.455"/>
<xacro:property name="base_height" value="0.237"/>
<xacro:property name="wheel_radius" value="0.0975"/>
<xacro:property name="wheel_width" value="0.05"/>
<xacro:property name="wheel_ygap" value="0.025"/>
<xacro:property name="wheel_zoff" value="0.05"/>
<xacro:property name="wheel_xoff" value="0.0"/>
<xacro:property name="caster_xoff" value="0.15"/>
<!-- Define some commonly used intertial properties -->
<xacro:macro name="box_inertia" params="m w h d">
<inertial>
<origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
<mass value="${m}"/>
<inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertia" params="m r h">
<inertial>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
<mass value="${m}"/>
<inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/>
</inertial>
</xacro:macro>
<xacro:macro name="sphere_inertia" params="m r">
<inertial>
<mass value="${m}"/>
<inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/>
</inertial>
</xacro:macro>
<!-- Robot Base -->
<link name="base_link">
<visual>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
<material name="Cyan">
<color rgba="0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<box size="${base_length} ${base_width} ${base_height}"/>
</geometry>
</collision>
<xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/>
</link>
<!-- Robot Footprint -->
<link name="base_footprint">
<xacro:box_inertia m="0" w="0" d="0" h="0"/>
</link>
<joint name="base_joint" type="fixed">
<parent link="base_link"/>
<child link="base_footprint"/>
<origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
</joint>
<!-- Wheels -->
<xacro:macro name="wheel" params="prefix x_reflect y_reflect">
<link name="${prefix}_link">
<visual>
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
<material name="Gray">
<color rgba="0.5 0.5 0.5 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
</geometry>
</collision>
<xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>
</link>
<joint name="${prefix}_joint" type="continuous">
<parent link="base_link"/>
<child link="${prefix}_link"/>
<origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
</xacro:macro>
<xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />
<xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" />
<!-- <link name="front_caster">
<visual>
<geometry>
<sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</geometry>
<material name="Cyan">
<color rgba="0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</geometry>
</collision>
<xacro:sphere_inertia m="0.5" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
</link> -->
<!-- <joint name="caster_joint" type="fixed">
<parent link="base_link"/>
<child link="front_caster"/>
<origin xyz="${caster_xoff} 0.0 ${-(base_height/2)}" rpy="0 0 0"/>
</joint> -->
<joint name="lidar_joint" type="fixed">
<parent link="base_link"/>
<child link="laser"/>
<origin xyz="0.14 0 ${base_height + 0.065}" rpy="0 0 0"/>
</joint>
<link name="laser">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="file://$(find pionier_description)/meshes/hokuyo.dae"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
</robot>

5
src/ros2aria/CMakeLists.txt
View File

@ -41,7 +41,7 @@ add_executable(ros2aria
src/wheels.cpp src/wheels.cpp
src/clutch.cpp src/clutch.cpp
src/state.cpp) src/state.cpp)
ament_target_dependencies(ros2aria rclcpp) ament_target_dependencies(ros2aria rclcpp)
ament_target_dependencies(ros2aria std_srvs) ament_target_dependencies(ros2aria std_srvs)
ament_target_dependencies(ros2aria geometry_msgs) ament_target_dependencies(ros2aria geometry_msgs)
@ -50,7 +50,8 @@ ament_target_dependencies(ros2aria nav_msgs)
ament_target_dependencies(ros2aria tf2) ament_target_dependencies(ros2aria tf2)
ament_target_dependencies(ros2aria ros2aria_msgs) ament_target_dependencies(ros2aria ros2aria_msgs)
target_include_directories(ros2aria PUBLIC target_include_directories(ros2aria PRIVATE
include
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include> $<INSTALL_INTERFACE:include>
/usr/local/Aria/include) /usr/local/Aria/include)

41
src/ros2aria/include/ros2aria/raiibot.hpp Normal file
View File

@ -0,0 +1,41 @@
#pragma once
#include <string>
#include <memory>
#include <iostream>
#include "Aria/Aria.h"
#include "rclcpp/rclcpp.hpp"
using namespace std::chrono_literals;
class RAIIBot
{
public:
typedef std::shared_ptr<RAIIBot> SharedPtr;
RAIIBot(rclcpp::Node *node, std::string port);
~RAIIBot();
ArRobot *getRobot();
ArGripper *getGripper();
rclcpp::Clock::SharedPtr getClock();
void lock();
void unlock();
void pokeWatchdog();
private:
ArRobot *robot = nullptr;
ArRobotConnector *robotConn = nullptr;
ArArgumentBuilder *args = nullptr;
ArArgumentParser *argparser = nullptr;
ArGripper *gripper = nullptr;
rclcpp::Node *node = nullptr;
rclcpp::TimerBase::SharedPtr watchdogTimer;
rclcpp::Time lastWatchdogPoke;
rclcpp::Clock::SharedPtr clock;
rclcpp::Logger get_logger();
void startWatchdog();
void watchdog_cb();
};

29
src/ros2aria/src/ros2aria.hpp → src/ros2aria/include/ros2aria/ros2aria.hpp
View File

@ -1,18 +1,20 @@
#include "rclcpp/rclcpp.hpp"
#include "Aria/Aria.h" #include "Aria/Aria.h"
#include "geometry_msgs/msg/transform.hpp"
#include "sensor_msgs/msg/point_cloud.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/odometry.hpp"
#include "std_srvs/srv/empty.hpp" #include "raiibot.hpp"
#include "std_msgs/msg/bool.hpp" #include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int8.hpp"
#include "std_msgs/msg/float32.hpp"
#include "ros2aria_msgs/msg/robot_info_msg.hpp" #include "ros2aria_msgs/msg/robot_info_msg.hpp"
#include "./raiibot.cpp" #include "sensor_msgs/msg/joint_state.hpp"
#include "sensor_msgs/msg/point_cloud.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "std_msgs/msg/bool.hpp"
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/int8.hpp"
#include "std_srvs/srv/empty.hpp"
#include "tf2/transform_datatypes.h"
#include "tf2/utils.h"
#include "tf2_ros/transform_broadcaster.h"
#define UNUSED(x) (void)(x) #define UNUSED(x) (void)(x)
@ -43,6 +45,9 @@ private:
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pose_pub_; rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pose_pub_;
void publishPose(nav_msgs::msg::Odometry pose); void publishPose(nav_msgs::msg::Odometry pose);
void sendOdomTransform(rclcpp::Time stamp, tf2::Transform transform);
std::unique_ptr<tf2_ros::TransformBroadcaster> odom_tf_broadcaster_;
sensor_msgs::msg::JointState handleWheels(rclcpp::Time stamp); sensor_msgs::msg::JointState handleWheels(rclcpp::Time stamp);
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr wheels_pub_; rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr wheels_pub_;
void publishWheels(sensor_msgs::msg::JointState wheels); void publishWheels(sensor_msgs::msg::JointState wheels);
@ -61,7 +66,7 @@ private:
// services // services
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_; rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;
void stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const; void stop(const std_srvs::srv::Empty::Response::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const;
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr gripper_open_service_; rclcpp::Service<std_srvs::srv::Empty>::SharedPtr gripper_open_service_;
void gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const; void gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const;

2
src/ros2aria/package.xml
View File

@ -17,6 +17,8 @@
<depend>geometry_msgs</depend> <depend>geometry_msgs</depend>
<depend>sensor_msgs</depend> <depend>sensor_msgs</depend>
<depend>nav_msgs</depend> <depend>nav_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>ros2aria_msgs</depend> <depend>ros2aria_msgs</depend>
<export> <export>

2
src/ros2aria/src/clutch.cpp
View File

@ -1,4 +1,4 @@
#include "ros2aria.hpp" #include "ros2aria/ros2aria.hpp"
void Ros2Aria::clutch_callback(const std_msgs::msg::Bool::SharedPtr msg) void Ros2Aria::clutch_callback(const std_msgs::msg::Bool::SharedPtr msg)
{ {

5
src/ros2aria/src/cmd_vel.cpp
View File

@ -1,4 +1,4 @@
#include "ros2aria.hpp" #include "ros2aria/ros2aria.hpp"
void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{ {
@ -14,7 +14,8 @@ void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
auto r = robot->getRobot(); auto r = robot->getRobot();
r->setVel(x * 1e3); r->setVel(x * 1e3);
if (r->hasLatVel()) if (r->hasLatVel()){
r->setLatVel(y * 1e3); r->setLatVel(y * 1e3);
}
r->setRotVel(z * 180 / M_PI); r->setRotVel(z * 180 / M_PI);
} }

2
src/ros2aria/src/gripper.cpp
View File

@ -1,4 +1,4 @@
#include "./ros2aria.hpp" #include "ros2aria/ros2aria.hpp"
void Ros2Aria::gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const void Ros2Aria::gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
{ {

2
src/ros2aria/src/main.cpp
View File

@ -1,7 +1,7 @@
#include <cstdio> #include <cstdio>
#include <memory> #include <memory>
#include "ros2aria.hpp" #include "ros2aria/ros2aria.hpp"
int main(int argc, char **argv) int main(int argc, char **argv)
{ {

19
src/ros2aria/src/pose.cpp
View File

@ -1,6 +1,5 @@
#include "ros2aria.hpp" #include "ros2aria/ros2aria.hpp"
#include "tf2/utils.h"
#include "tf2/transform_datatypes.h"
inline void convert_transform_to_pose(const tf2::Transform &trans, geometry_msgs::msg::Pose &pose) inline void convert_transform_to_pose(const tf2::Transform &trans, geometry_msgs::msg::Pose &pose)
{ {
@ -30,13 +29,23 @@ nav_msgs::msg::Odometry Ros2Aria::handlePose(rclcpp::Time stamp)
pose.twist.twist.linear.y = r->getLatVel() / 1000; pose.twist.twist.linear.y = r->getLatVel() / 1000;
pose.twist.twist.angular.z = r->getRotVel() * M_PI / 180; pose.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
pose.header.frame_id = "header.frame_id"; // TODO: use correct frame_id pose.header.frame_id = "odom"; // TODO: use correct frame_id
pose.child_frame_id = "child_frame_id"; // TODO: use correct child_frame_id pose.child_frame_id = "base_link"; // TODO: use correct child_frame_id
pose.header.stamp = stamp; pose.header.stamp = stamp;
sendOdomTransform(stamp, geometry_msgs::msg::Transform(rotation, position));
return pose; return pose;
} }
void Ros2Aria::sendOdomTransform(rclcpp::Time stamp, tf2::Transform transform){
geometry_msgs::msg::TransformStamped t;
t.header.stamp = stamp;
t.header.frame_id = "odom";
t.child_frame_id = "base_link";
t.transform = transform;
odom_tf_broadcaster_->sendTransform(t);
}
void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose) void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose)
{ {
if (this->pose_pub_->get_subscription_count() == 0) if (this->pose_pub_->get_subscription_count() == 0)

2
src/ros2aria/src/publish.cpp
View File

@ -1,4 +1,4 @@
#include "ros2aria.hpp" #include "ros2aria/ros2aria.hpp"
void Ros2Aria::publish() void Ros2Aria::publish()
{ {

208
src/ros2aria/src/raiibot.cpp
View File

@ -1,143 +1,105 @@
#include <string> #include "ros2aria/raiibot.hpp"
#include <memory>
#include <iostream>
#include "Aria/Aria.h"
#include "rclcpp/rclcpp.hpp"
using namespace std::chrono_literals; using namespace std::chrono_literals;
class RAIIBot RAIIBot::RAIIBot(rclcpp::Node *node, std::string port) {
{ this->robot = new ArRobot();
public: this->gripper = new ArGripper(this->robot);
typedef std::shared_ptr<RAIIBot> SharedPtr; this->node = node;
this->clock = rclcpp::Clock::make_shared();
RAIIBot(rclcpp::Node *node, std::string port) args = new ArArgumentBuilder();
{ this->argparser = new ArArgumentParser(args);
this->robot = new ArRobot(); argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
this->gripper = new ArGripper(this->robot); args->add("-robotPort /dev/ttyS0"); // pass robot's serial port to Aria // TODO: use `port` variable.
this->node = node; // args->add("-robotLogPacketsReceived"); // log received packets
this->clock = rclcpp::Clock::make_shared(); // args->add("-robotLogPacketsSent"); // log sent packets
// args->add("-robotLogVelocitiesReceived"); // log received velocities
args = new ArArgumentBuilder(); // args->add("-robotLogMovementSent");
this->argparser = new ArArgumentParser(args); // args->add("-robotLogMovementReceived");
argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx) this->robotConn = new ArRobotConnector(argparser, robot);
args->add("-robotPort /dev/ttyS0"); // pass robot's serial port to Aria // TODO: use `port` variable. if (!this->robotConn->connectRobot()) {
// args->add("-robotLogPacketsReceived"); // log received packets RCLCPP_ERROR(this->get_logger(), "RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
// args->add("-robotLogPacketsSent"); // log sent packets // rclcpp::shutdown();
// args->add("-robotLogVelocitiesReceived"); // log received velocities
// args->add("-robotLogMovementSent");
// args->add("-robotLogMovementReceived");
this->robotConn = new ArRobotConnector(argparser, robot);
if (!this->robotConn->connectRobot())
{
// RCLCPP_INFO(this->get_logger(), "RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
// rclcpp::shutdown();
}
this->robot->runAsync(true);
this->robot->enableMotors();
this->startWatchdog();
} }
~RAIIBot() this->robot->runAsync(true);
{ this->robot->enableMotors();
std::cout << std::endl
<< "RAIIBOT DESTRUCTOR RUNNING!" << std::endl;
if (this->robot != nullptr) this->startWatchdog();
{ }
this->robot->lock();
std::cout << "disabling motors" << std::endl;
this->robot->disableMotors();
std::cout << "disabled motors" << std::endl;
Aria::shutdown();
}
if (this->robotConn != nullptr)
{
std::cout << "disconnecting" << std::endl;
this->robotConn->disconnectAll();
std::cout << "disconnected" << std::endl;
}
if (this->args != nullptr) RAIIBot::~RAIIBot() {
delete this->args; std::cout << std::endl
if (this->argparser != nullptr) << "RAIIBOT DESTRUCTOR RUNNING!" << std::endl;
delete this->argparser;
if (this->robotConn != nullptr) if (this->robot != nullptr) {
delete this->robotConn; this->robot->lock();
if (this->robot != nullptr) std::cout << "disabling motors" << std::endl;
delete this->robot; this->robot->disableMotors();
std::cout << "disabled motors" << std::endl;
Aria::shutdown();
}
if (this->robotConn != nullptr) {
std::cout << "disconnecting" << std::endl;
this->robotConn->disconnectAll();
std::cout << "disconnected" << std::endl;
} }
ArRobot *getRobot() if (this->args != nullptr)
{ delete this->args;
return this->robot; if (this->argparser != nullptr)
} delete this->argparser;
if (this->robotConn != nullptr)
delete this->robotConn;
if (this->robot != nullptr)
delete this->robot;
}
ArGripper *getGripper() ArRobot *RAIIBot::getRobot() {
{ return this->robot;
return this->gripper; }
}
rclcpp::Clock::SharedPtr getClock() ArGripper *RAIIBot::getGripper() {
{ return this->gripper;
return this->clock; }
}
void lock() rclcpp::Clock::SharedPtr RAIIBot::getClock() {
{ return this->clock;
if (this->robot != nullptr) }
this->robot->lock();
}
void unlock() void RAIIBot::lock() {
{ if (this->robot != nullptr)
if (this->robot != nullptr) this->robot->lock();
this->robot->unlock(); }
}
void pokeWatchdog() void RAIIBot::unlock() {
{ if (this->robot != nullptr)
// this should probably be made thread safe, or something? this->robot->unlock();
this->lastWatchdogPoke = this->clock->now(); }
}
private: void RAIIBot::pokeWatchdog() {
ArRobot *robot = nullptr; // this should probably be made thread safe, or something?
ArRobotConnector *robotConn = nullptr; this->lastWatchdogPoke = this->clock->now();
ArArgumentBuilder *args = nullptr; }
ArArgumentParser *argparser = nullptr;
ArGripper *gripper = nullptr;
rclcpp::Node *node = nullptr;
rclcpp::TimerBase::SharedPtr watchdogTimer;
rclcpp::Time lastWatchdogPoke;
rclcpp::Clock::SharedPtr clock;
rclcpp::Logger get_logger() rclcpp::Logger RAIIBot::get_logger() {
{ return this->node->get_logger();
return this->node->get_logger(); }
}
void startWatchdog() void RAIIBot::startWatchdog() {
{ this->lastWatchdogPoke = clock->now();
this->lastWatchdogPoke = clock->now(); watchdogTimer = this->node->create_wall_timer(0.1s, std::bind(&RAIIBot::watchdog_cb, this));
watchdogTimer = this->node->create_wall_timer(0.1s, std::bind(&RAIIBot::watchdog_cb, this)); }
}
void watchdog_cb() void RAIIBot::watchdog_cb() {
{ auto now = this->clock->now();
auto now = this->clock->now(); if (now - this->lastWatchdogPoke > 1s) {
if (now - this->lastWatchdogPoke > 1s) std::lock_guard<RAIIBot> lock(*this);
{ auto r = this->getRobot();
std::lock_guard<RAIIBot> lock(*this); r->setVel(0);
auto r = this->getRobot(); r->setRotVel(0);
r->setVel(0); if (r->hasLatVel())
r->setRotVel(0); r->setLatVel(0);
if (r->hasLatVel())
r->setLatVel(0);
}
} }
}; }

6
src/ros2aria/src/ros2aria.cpp
View File

@ -1,4 +1,4 @@
#include "ros2aria.hpp" #include "ros2aria/ros2aria.hpp"
using std::placeholders::_1; using std::placeholders::_1;
using std::placeholders::_2; using std::placeholders::_2;
@ -19,6 +19,7 @@ Ros2Aria::Ros2Aria()
sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10); sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10);
pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("pose", 1000); pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("pose", 1000);
odom_tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("wheels", 1000); wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("wheels", 1000);
@ -46,8 +47,7 @@ Ros2Aria::~Ros2Aria()
r->remSensorInterpTask("ROSPublishingTask"); r->remSensorInterpTask("ROSPublishingTask");
} }
void Ros2Aria::stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const void Ros2Aria::stop(const std_srvs::srv::Empty::Response::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const {
{
UNUSED(request); UNUSED(request);
UNUSED(response); UNUSED(response);
RCLCPP_INFO(this->get_logger(), "stop"); RCLCPP_INFO(this->get_logger(), "stop");

2
src/ros2aria/src/sonar.cpp
View File

@ -1,4 +1,4 @@
#include "ros2aria.hpp" #include "ros2aria/ros2aria.hpp"
#include "sensor_msgs/point_cloud_conversion.hpp" #include "sensor_msgs/point_cloud_conversion.hpp"

6
src/ros2aria/src/state.cpp
View File

@ -1,4 +1,4 @@
#include "ros2aria.hpp" #include "ros2aria/ros2aria.hpp"
void Ros2Aria::publishState(rclcpp::Time stamp) void Ros2Aria::publishState(rclcpp::Time stamp)
{ {
@ -31,9 +31,7 @@ void Ros2Aria::publishState(rclcpp::Time stamp)
robot_info_msg.twist.linear.x = r->getVel() / 1000; robot_info_msg.twist.linear.x = r->getVel() / 1000;
robot_info_msg.twist.linear.y = r->getLatVel() / 1000.0; robot_info_msg.twist.linear.y = r->getLatVel() / 1000.0;
robot_info_msg.twist.angular.z = r->getRotVel() * M_PI / 180; robot_info_msg.twist.angular.z = r->getRotVel() * M_PI / 180;
// TODO: actually keep track of robot state
// TODO: actually keep track of robot state (true -> unlocked, false ->
// locked). This requires safety plugin to be implemented
robot_info_msg.state.data = true; robot_info_msg.state.data = true;
robot_info_msg.clutch.data = r->areMotorsEnabled(); robot_info_msg.clutch.data = r->areMotorsEnabled();
// TODO: actually look for obstacles // TODO: actually look for obstacles

2
src/ros2aria/src/wheels.cpp
View File

@ -1,4 +1,4 @@
#include "ros2aria.hpp" #include "ros2aria/ros2aria.hpp"
sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp) sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp)
{ {