Skip to content

Commit 3383fde

Browse files
author
Tim Patten
committed
Saving changes on robotino
1 parent c9a36f3 commit 3383fde

File tree

12 files changed

+327
-0
lines changed

12 files changed

+327
-0
lines changed

Diff for: ‎squirrel_3d_mapping/launch/3d_mapping_initial.launch

+40
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
<?xml version="1.0" encoding="utf-8"?>
2+
<launch>
3+
<arg name="input_topic" default="/kinect/depth/points"/>
4+
<arg name="base_frame" default="/base_link"/>
5+
<arg name="output_binary" default="/octomap_binary"/>
6+
<arg name="output_full" default="/octomap_full"/>
7+
<arg name="map_file" default="$(find squirrel_navigation)/octomaps/default-octomap.bt"/>
8+
<node pkg="squirrel_3d_mapping" type="octomap_server_node"
9+
name="squirrel_3d_mapping" output="screen">
10+
<param name="resolution" value="0.05" />
11+
<param name="frame_id" type="string" value="map" />
12+
<param name="max_sensor_range" value="4.0" />
13+
<param name="topic_binary" value="$(arg output_binary)" />
14+
<param name="topic_full" value="$(arg output_full)" />
15+
<param name="filter_ground" value="true" />
16+
<param name="base_frame_id" value="$(arg base_frame)" />
17+
<param name="pointcloud_min_z" value="-0.00" />
18+
<param name="pointcloud_max_z" value="0.70" />
19+
<param name="pointcloud_min_y" value="-1.0" />
20+
<param name="pointcloud_max_y" value="1.0" />
21+
<param name="pointcloud_min_x" value="0.0" />
22+
<param name="pointcloud_max_x" value="2.5" />
23+
<param name="ground_filter/plane_distance" value="0.03" />
24+
<param name="update_rate_hz" value="40.0" />
25+
<param name="voxel_filter/enabled" value="false" />
26+
<param name="voxel_filter/voxel_size" value="0.025" />
27+
<param name="distance_transform/enabled" value="false" />
28+
<param name="distance_transform/max_dist" value="0.25" />
29+
<param name="distance_transform/unknown_as_occupied" value="false" />
30+
<param name="distance_transform/min_x" value="-5.0" />
31+
<param name="distance_transform/max_x" value="5.0" />
32+
<param name="distance_transform/min_y" value="-5.0" />
33+
<param name="distance_transform/max_y" value="5.0" />
34+
<param name="distance_transform/min_z" value="0.0" />
35+
<param name="distance_transform/max_z" value="0.9" />
36+
<!-- <param name="latch" value="false" /> -->
37+
<remap from="/cloud_in" to="$(arg input_topic)" />
38+
</node>
39+
</launch>
40+

Diff for: ‎squirrel_footprint_observer/config/arm_folding_observer_params.yaml

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
verbose: true
2+
joint_states_topic: /arm_controller/joint_states
3+
joint_names: [arm_joint1, arm_joint2, arm_joint3, arm_joint4, arm_joint5]
4+
ref_joint_states: [0.292, 2.27, 0.19, -1.17, 0.53]

Diff for: ‎squirrel_footprint_observer/include/squirrel_footprint_observer/arm_folding_observer.h

+73
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
// Copyright (C) 2017 Federico Boniardi and Wolfram Burgard
2+
//
3+
// This program is free software: you can redistribute it and/or modify
4+
// it under the terms of the GNU General Public License as published by
5+
// the Free Software Foundation, either version 3 of the License, or
6+
// (at your option) any later version.
7+
//
8+
// This program is distributed in the hope that it will be useful,
9+
// but WITHOUT ANY WARRANTY; without even the implied warranty of
10+
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11+
// GNU General Public License for more details.
12+
//
13+
// You should have received a copy of the GNU General Public License
14+
// along with this program. If not, see <http://www.gnu.org/licenses/>.
15+
16+
#ifndef SQUIRREL_FOOTPRINT_OBSERVER_ARM_FOLDING_OBSERVER_H_
17+
#define SQUIRREL_FOOTPRINT_OBSERVER_ARM_FOLDING_OBSERVER_H_
18+
19+
#include <sensor_msgs/JointState.h>
20+
21+
#include <ros/init.h>
22+
#include <ros/subscriber.h>
23+
#include <ros/node_handle.h>
24+
25+
#include <map>
26+
#include <string>
27+
#include <vector>
28+
29+
namespace squirrel_footprint_observer {
30+
namespace {
31+
32+
constexpr double kJointValueTolerance = 1e-1;
33+
34+
}
35+
36+
class ArmFoldingObserver {
37+
public:
38+
ArmFoldingObserver() : joint_index_map_(nullptr) {}
39+
virtual ~ArmFoldingObserver() {}
40+
41+
void initialize(const std::string& name = "");
42+
43+
inline bool folded() const { return !plan_with_footprint_; }
44+
45+
// Spinners.
46+
inline void spin() { ros::spin(); }
47+
inline void spinOnce() { ros::spinOnce(); }
48+
49+
private:
50+
void jointStatesCallback(
51+
const sensor_msgs::JointState::ConstPtr& joint_state);
52+
53+
void toggleFootprintPlanner(bool on_off) const;
54+
void buildJointIndexMap(const std::vector<std::string>& joint_msg_names);
55+
56+
private:
57+
bool verbose_;
58+
std::string joint_states_topic_;
59+
std::map<std::string, double> joints_values_;
60+
61+
bool plan_with_footprint_;
62+
std::unique_ptr<std::map<std::string, int>> joint_index_map_;
63+
64+
std::map<std::string, bool> joints_exists_;
65+
66+
ros::Subscriber joint_states_sub_;
67+
68+
ros::NodeHandle gnh_;
69+
};
70+
71+
} // namespace squirrel_footprint_observer
72+
73+
#endif /* SQUIRREL_FOOTPRINT_OBSERVER_ARM_FOLDING_OBSERVER_H_ */

Diff for: ‎squirrel_footprint_observer/launch/arm_folding_observer.launch

+9
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<launch>
2+
<arg name="config_file" default="$(find squirrel_footprint_observer)/config/arm_folding_observer_params.yaml"/>
3+
4+
<node name="arm_folding_observer" pkg="squirrel_footprint_observer"
5+
type="arm_folding_observer_node" output="screen">
6+
<rosparam file="$(arg config_file)" command="load"/>
7+
</node>
8+
9+
</launch>

Diff for: ‎squirrel_footprint_observer/src/arm_folding_observer.cpp

+137
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,137 @@
1+
// Copyright (C) 2017 Federico Boniardi and Wolfram Burgard
2+
//
3+
// This program is free software: you can redistribute it and/or modify
4+
// it under the terms of the GNU General Public License as published by
5+
// the Free Software Foundation, either version 3 of the License, or
6+
// (at your option) any later version.
7+
//
8+
// This program is distributed in the hope that it will be useful,
9+
// but WITHOUT ANY WARRANTY; without even the implied warranty of
10+
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11+
// GNU General Public License for more details.
12+
//
13+
// You should have received a copy of the GNU General Public License
14+
// along with this program. If not, see <http://www.gnu.org/licenses/>.
15+
16+
#include "squirrel_footprint_observer/arm_folding_observer.h"
17+
18+
#include <dynamic_reconfigure/BoolParameter.h>
19+
#include <dynamic_reconfigure/Config.h>
20+
#include <dynamic_reconfigure/Reconfigure.h>
21+
22+
#include <ros/service.h>
23+
24+
#include <iomanip>
25+
#include <stdexcept>
26+
#include <utility>
27+
28+
namespace squirrel_footprint_observer {
29+
30+
void ArmFoldingObserver::initialize(const std::string &name) {
31+
ros::NodeHandle nh("~/" + name);
32+
33+
std::vector<std::string> joint_names;
34+
std::vector<double> joint_values;
35+
36+
nh.param<bool>("verbose", verbose_, false);
37+
nh.param<std::string>("joint_states_topic", joint_states_topic_, "/states");
38+
nh.param<std::vector<std::string>>("joint_names", joint_names, {});
39+
nh.param<std::vector<double>>("ref_joint_states", joint_values, {});
40+
41+
if (joint_names.size() != joint_values.size()) {
42+
ROS_ERROR_STREAM_NAMED(
43+
"arm_folding_observer",
44+
"'joint_names' and 'ref_joint_states' not of the same size.");
45+
ros::shutdown();
46+
}
47+
48+
// Zip the vectors.
49+
for (unsigned int i = 0; i < joint_names.size(); ++i)
50+
joints_values_.emplace(joint_names[i], joint_values[i]);
51+
52+
// Subscribe to the joint states.
53+
joint_states_sub_ = gnh_.subscribe(
54+
joint_states_topic_, 1, &ArmFoldingObserver::jointStatesCallback, this);
55+
}
56+
57+
void ArmFoldingObserver::jointStatesCallback(
58+
const sensor_msgs::JointState::ConstPtr &joint_state) {
59+
if (verbose_)
60+
ROS_INFO_STREAM_ONCE_NAMED("arm_folding_observer",
61+
"Subscribed to '" << joint_states_topic_
62+
<< "'.");
63+
if (!joint_index_map_)
64+
buildJointIndexMap(joint_state->name);
65+
66+
gnh_.getParamCached("/move_base/GlobalPlanner/plan_with_footprint",
67+
plan_with_footprint_);
68+
69+
const auto &positions = joint_state->position;
70+
71+
bool plan_with_footprint = false;
72+
for (const auto &joint_value : joints_values_) {
73+
if (!joints_exists_[joint_value.first])
74+
continue;
75+
76+
const std::string &joint_name = joint_value.first;
77+
const double joint_ref_value = joint_value.second;
78+
const int joint_index = joint_index_map_->at(joint_name);
79+
const double joint_cur_value = positions[joint_index];
80+
81+
if (std::abs(joint_ref_value - joint_cur_value) > kJointValueTolerance) {
82+
plan_with_footprint = true;
83+
break;
84+
}
85+
}
86+
87+
if (plan_with_footprint != plan_with_footprint_)
88+
toggleFootprintPlanner(plan_with_footprint);
89+
}
90+
91+
void ArmFoldingObserver::toggleFootprintPlanner(bool on_off) const {
92+
dynamic_reconfigure::BoolParameter bool_param;
93+
bool_param.name = "plan_with_footprint";
94+
bool_param.value = on_off;
95+
96+
dynamic_reconfigure::Reconfigure reconfigure;
97+
reconfigure.request.config.bools.emplace_back(bool_param);
98+
99+
ros::service::waitForService("/move_base/GlobalPlanner/set_parameters");
100+
101+
if (ros::service::call("/move_base/GlobalPlanner/set_parameters",
102+
reconfigure)) {
103+
ROS_INFO_STREAM_COND_NAMED(verbose_, "arm_folding_observer",
104+
"Set 'plan_with_footprint' to " << std::boolalpha
105+
<< on_off);
106+
} else {
107+
ROS_WARN_STREAM_NAMED("arm_folding_observer",
108+
"Tried to set 'plan_with_footprint' to"
109+
<< std::boolalpha << on_off
110+
<< ". Didn't work...");
111+
}
112+
}
113+
114+
void ArmFoldingObserver::buildJointIndexMap(
115+
const std::vector<std::string> &joint_msg_names) {
116+
joint_index_map_.reset(new std::map<std::string, int>);
117+
118+
const auto begin = joint_msg_names.begin();
119+
const auto end = joint_msg_names.end();
120+
121+
for (const auto &joint_value : joints_values_) {
122+
const auto &joint_name = joint_value.first;
123+
124+
auto it = std::find(begin, end, joint_name);
125+
126+
if (it == end) {
127+
ROS_WARN_STREAM_NAMED("arm_folding_observer",
128+
joint_name << "does not appear to be published.");
129+
joints_exists_.emplace(joint_name, false);
130+
} else {
131+
joint_index_map_->emplace(joint_name, (int)std::distance(begin, it));
132+
joints_exists_.emplace(joint_name, true);
133+
}
134+
}
135+
}
136+
137+
} // namespace squirrel_footprint_observer

Diff for: ‎squirrel_footprint_observer/src/arm_folding_observer_node.cpp

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
// Copyright (C) 2017 Federico Boniardi and Wolfram Burgard
2+
//
3+
// This program is free software: you can redistribute it and/or modify
4+
// it under the terms of the GNU General Public License as published by
5+
// the Free Software Foundation, either version 3 of the License, or
6+
// (at your option) any later version.
7+
//
8+
// This program is distributed in the hope that it will be useful,
9+
// but WITHOUT ANY WARRANTY; without even the implied warranty of
10+
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11+
// GNU General Public License for more details.
12+
//
13+
// You should have received a copy of the GNU General Public License
14+
// along with this program. If not, see <http://www.gnu.org/licenses/>.
15+
16+
#include "squirrel_footprint_observer/arm_folding_observer.h"
17+
18+
#include <cstdlib>
19+
20+
int main(int argc, char *argv[]) {
21+
ros::init(argc, argv, "arm_folding_observer");
22+
23+
squirrel_footprint_observer::ArmFoldingObserver afo;
24+
afo.initialize();
25+
afo.spin();
26+
27+
return EXIT_SUCCESS;
28+
}

Diff for: ‎squirrel_navigation/maps/ikea.pgm

+5
Large diffs are not rendered by default.

Diff for: ‎squirrel_navigation/maps/ikea.yaml

+7
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
image: ikea.pgm
2+
resolution: 0.050000
3+
origin: [-10.000000, -10.000000, 0.000000]
4+
negate: 0
5+
occupied_thresh: 0.65
6+
free_thresh: 0.196
7+

Diff for: ‎squirrel_navigation/maps/vienna_integration.pgm

+5
Large diffs are not rendered by default.

Diff for: ‎squirrel_navigation/maps/vienna_integration.yaml

+7
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
image: vienna_integration.pgm
2+
resolution: 0.050000
3+
origin: [-100.000000, -100.000000, 0.000000]
4+
negate: 0
5+
occupied_thresh: 0.65
6+
free_thresh: 0.196
7+

Diff for: ‎squirrel_navigation/maps/vienna_planet.pgm

+5
Large diffs are not rendered by default.

Diff for: ‎squirrel_navigation/maps/vienna_planet.yaml

+7
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
image: vienna_planet.pgm
2+
resolution: 0.050000
3+
origin: [-100.000000, -100.000000, 0.000000]
4+
negate: 0
5+
occupied_thresh: 0.65
6+
free_thresh: 0.196
7+

0 commit comments

Comments
 (0)